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[57] ABSTRACT 

A robot manipulator controller for a flexible manipulator 
arm having plural bodies connected at respective movable 
hinges and flexible in plural deformation modes correspond- 
ing to respective modal spatial influence vectors relating 
deformations of plural spaced nodes of respective bodies to 
the plural deformation modes, operates by computing articu- 
lated body quantities for each of the bodies from respective 
modal spatial influence vectors, obtaining specified body 
forces for each of the bodies, and computing modal defor- 
mation accelerations of the nodes and hinge accelerations of 
the hinges from the specified body forces, from the articu- 
lated body quantities and from the modal spatial influence 
vectors. In one embodiment of the invention, the controller 
further operates by comparing the accelerations thus com- 
puted to desired manipulator motion to determine a motion 
discrepancy, and correcting the specified body forces so as 
to reduce the motion discrepancy. The manipulator bodies 
and hinges are characterized by respective vectors of defor- 
mation and hinge configuration variables, and computing 
modal deformation accelerations and hinge accelerations is 
carried out for each one of the bodies beginning with the 
outermost body by computing a residual body force from a 
residual body force of a previous body and from the vector 
of deformation and hinge configuration variables, comput- 
ing a resultant hinge acceleration from the body force, the 
residual body force and the articulated hinge inertia, and 
revising the residual body force modal body acceleration. 

45 Claims, 7 Drawing Sheets 
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DEFORMATION / HINGE CONFIGURATION VECTOR 


OBTAIN BODY FORCE ON CURRENT BODY 


COMPUTE RESULTANT HINGE FORCE FROM BODY FORCE 
AND FROM RESIDUAL FORCE BODY FORCE 


COMPUTE RESULTANT HINGE ACCELERATION FROM 
RESULTANT HINGE FORCE AND HINGE INERTIA 
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CONTROLLING FLEXIBLE ROBOT ARMS 
USING HIGH SPEED DYNAMICS PROCESS 

ORIGIN OF THE INVENTION 

The invention described herein was made in the perfor- 
mance of work under a NASA contract, and is subject to the 
provisions of Public Law 96-517 (35 USC 202) in which the 
contractor has elected not to retain title. 

BACKGROUND OF THE INVENTION 

1. Technical Field 

The invention relates to robot manipulators and more 
particularly to a method and apparatus for controlling robot 
arms having flexible links using a high speed recursive 
dynamics algorithm to solve for the accelerations of link 
deformation and hinge rotations from specified body forces 
applied to the links. 

2. Background Art 

Controlling robot manipulator arms is a well-known prob- 
lem and has been described in a number of publications. The 
invention herein will be described with reference to the 
following publications by referring to each publication by 
number, such as Ref. [1], Ref. [2], or simply [1] or [2], for 
example. 

References 

[1] Rodriguez, G., Kreutz, K., and Jain, A., “A Spatial 
Operator Algebra for Manipulator Modeling and Con- 
trol,” The International Journal of Robotics Research , 
Vol. 10, No. 4, August 1991, pp. 371-381. 

[2] Jain, A., “Unified Formulation of Dynamics for Serial 
Rigid Multibody Systems,” Journal of Guidance, Con- 
trol and Dynamics, Vol. 14, No. 3, May-June 1991, pp. 
531-542. 

[3] Kim, S. S. and Haug, E. J., “A Recursive Formulation 
for Flexible Multibody Dynamics, Part I: Open-Loop 
Systems,” Computer Methods in Applied Mechanics 
and Engineering, Vol. 71, No. 3, 1988, pp. 293-314. 

[4] Changizi, K. and Shabana, A. A., “A Recursive 
Formulation for the Dynamic Analysis of Open Loop 
Deformable Multibody Systems,” ASME Jl of Applied 
Mechanics, Vol. 55, No. 3, September 1988, pp. 
687-693. 

[5] Keat, J. E., “Multibody System Order n Dynamics 
Formulation Based in Velocity Transform Method,” 
Journal of Guidance, Control and Dynamics, Vol. 13, 
No. 2, March-April 1990. 
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H. P. “A Digital Computer Program for the Dynamic 
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COS),” NASA Technical Paper 1219, NASA, May 
1978. 
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pp. 584-590. 
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6, December 1987, pp. 624-639. 

[9] Likins, P. W., “Modal Method for Analysis of Free 
Rotations of Spacecraft,” AIAA Journal , Vol. 5, July 
1967, pp. 1304-1308. 
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JPL Publication 90-26, Jet Propulsion Laboratory, 
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[12] Walker, M. W. and Orin, D. E., “Efficient Dynamic 
Computer Simulation of Robotic Mechanisms,” ASME 
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trol, Vol. 104, No. 3, September 1982, pp. 205-211. 
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“On-line Computational Scheme for Mechanical 
Manipulators,” ASME Journal of Dynamic Systems, 

15 Measurement, and Control , Vol. 102, No. 2, June 1980, 
pp. 69-76. 

[14] Jain, A. and Rodriguez, G., “Kinematics and Dynam- 
ics of Under-Actuated Manipulators,” in IEEE Inter- 
national Conference on Robotics and Automation, Sac- 

20 ramento, Calif., April 1991. 

[15] Featherstone, R., “The Calculation of Robot Dynam- 
ics using Articulated-Body Inertias,” The International 
Journal of Robotics Research, Vol. 2, No. 1, Spring 
1983, pp. 13-30. 

25 [16] Padilla, C. E. and von Flotow, A. H., “Nonlinear 

Strain-Displacement Relations and Flexible Multibody 
Dynamics,” in Proceedings of the 3rd Annual Confer- 
ence on Aerospace Computational Control, Vol. 1, 
Oxnard, Calif., pp. 230-245, August 1989. (JPL Pub- 
30 lication 89-45, Jet Propulsion Laboratory, Pasadena, 
Calif., 1989). 

[17] Kane, T. R., Ryan, R. R., and Banerjee, A. K., 
“Dynamics of a Cantilevered Beam attached to a Mov- 
ing Base,” Journal of Guidance, Control and Dynam- 
35 ics , Vol. 10, No. 2, March-April 1987, pp. 139-151. 

The invention uses spatial operators to develop new 
spatially recursive dynamics algorithms for flexible multi- 
body systems. The operator description of the dynamics is 
identical to that for rigid multibody systems. Assumed-mode 
40 models are used for the deformation of each individual body. 
The algorithms are based on two spatial operator factoriza- 
tions of the system mass matrix. The first (Newton-Euler) 
factorization of the mass matrix leads to recursive algo- 
rithms for the inverse dynamics, mass matrix evaluation, and 
45 composite-body forward dynamics for the system. The sec- 
ond (Innovations) factorization of the mass matrix, leads to 
an operator expression for the mass matrix inverse and to a 
recursive articulated-body forward dynamics algorithm. The 
primary focus is on serial chains, but extensions to general 
50 topologies are also described. A comparison of computa- 
tional costs shows that the articulated-body forward dynam- 
ics algorithm is much more efficient than the composite- 
body algorithm for most flexible multibody systems. 

1. Nomenclature 

55 We use coordinate-free spatial notation ([1, 2]) in this 
specification. A spatial velocity of a frame is a 6-dimensional 
quantity whose upper 3 elements are the angular velocity 
and whose lower 3 elements are the linear velocity. A spatial 
force is a 6-dimensional quantity whose upper 3 elements 
60 are a moment vector and whose lower 3 elements are a force 
vector. 

A variety of indices are used to identify different spatial 
quantities. Some examples are: V^*) is the spatial velocity 
of the } th node on the k th body; V/k)=col{V,(j*)} is the 
65 composite vector of spatial velocities of all the nodes on the 
k' A body; V/=col{V,(k)} is the vector of spatial velocities of 
all the nodes for all the bodies in the serial chain. The index 
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k will be used to refer to both the k' /z body as well as the k' /l 
body reference frame T k , with the usage being apparent 
from the context. Some key quantities are defined below in 
accordance with FIGS, la and lb. 

General Quantities 5 

x=[X] x e & 3x3 — the skew-symmetric cross-product 
matrix associated with the 3-dimensional vector x 



the time derivative of x with respect to an inertial frame 
X — the time derivative of x with respect to the body-fixed 
(rotating) frame 

diag{x(k)} — a block diagonal matrix whose k' /2 diagonal 15 
element is x(k) 

col{x(k)} — a column vector whose k th element is x(k) 
l(x,y) e ft 3 — the vector from point/frame x to point/frame 

y 



the spatial transformation operator which transforms 
spatial velocities and forces between points/frames x 25 
and y 

Individual Body Nodal Data 

n 5 (k) — number of nodes on the k' /l body 
T k — body reference frame with respect to which the 
deformation field for the ]a th body is measured. The 30 
motion of this frame characterizes the motion of the k //l 
body as a rigid body. 

\ k — ] th node on the k th body 

lo(kjjt) e St 3 — vector from ? k to the location (before 
deformation) of the ) th node reference frame on the k lh 35 
body 

8 z (j*) e ft 3 — translational deformation of the ] th node on 
the k /;i body 

l(k4H 0 (kj J .)+5,(j*) e gt 3 — vector from r k to the loca- 
tion (after deformation) of the ] th node reference frame 
on the k th body 

5 ra (i f) e ft 3 — deformation angul ar velocity of the j th node 
on the k th body with respect to the body frame T k 
5 v (jjt) e Si 3 — deformation linear velocity of the ] lh node 45 
on the k' A body with respect to the body frame T k 
u(j fc ) e ft 6 — the spatial displacement of node j*. The 
translational component of u(j A ) is 5 z (j*), while its time 
derivative with respect to the body frame T k is 



e ft 3x3 — inertia tensor about the nodal reference 
frame for the j th node on the k th body 
p(j*) e ft 3 — vector from the nodal reference frame to the 
node center of mass for the \ th node on the k ih body 
m(j A .) — mass of the ] th node of the k th body 


M s (j k ) = 


( T(/ * ) 

\ -m(jk)p(jk) 


m (jk)p(jk) \ ft 6x6 
m(j k )l ) 


60 


spatial inertia about the nodal reference frame for the f l 
node on the k^ body 65 

M,.(k) diagfM/j*)} e ft 6 " s(fc)x6nsW — structural mass 
matrix for the k" 1 body 


4 

K v (k) e ft 6ns( * )x6ns(/:) — structural stiffness matrix for the 
k th body 

Individual Body Modal Data 

n m (k) — number of assumed modes for the k th body 

Jf (k)=n m (k)+6 — number of deformation plus rigid-body 
degrees of freedom for the k /ft body 

T|(k) e ft" m(&) — vector of modal deformation variables for 
the k th body 

n/(k) e ft 6 — modal spatial displacement vector for the 
X th mode at the ] k h nodal reference frame 

IF'(k)=[n/(k), . . . , H„ m( */(k)]e ft 6xn ” (fc) — modal spatial 
influence vector for the ] k h node. The spatial deforma- 
tion of node j k is given by u(j A )=IF(k)r|(k). 

II(k)=col{IF(k)}e ft 6/Is(/c)x ” inW — the modal matrix for the 
k f/l body. The I th column of H(k) is denoted II r (k) e 
ft 6 ” sCfc) and is the mode shape function for the r th 
assumed mode for the k /A body. The deformation field 
for the k"' body is given by u(k)=ll(k)rj(k), while 

(k)=n(k)ri(k). 

M m (k) e ft TF (k)x Jl (k) — modal mass matrix for the 
k th body. 

K m (k) e ft Jl (k)x Jf (k) — modal stiffness matrix for the 
k //2 body. 

Multibody Data 

N — number of bodies in the serial flexible multibody 
system 


overall degrees of freedom in the serial chain obtain by 
disregarding the hinge constraints 
n r (k) — number of degrees of freedom for the k r/z hinge 
A^(k)=n m (k)+n r (k) — number of deformation plus hinge 
degrees of freedom for the k //z body 


k=l 


(ft)- 


overall deformation plus hinge degrees of freedom for 
the serial chain. 

d A — node on the k r; ' body to which the k* /l hinge is 
attached 

t k — node on the k lh body to which the (k-l) //2 hinge is 
attached 

O k — reference frame for the k lh hinge on the k th body. 
This frame is fixed to node d k . 

O k — ^reference frame for the k th hinge on the (k+l) /ft 
body. This frame is fixed to node t k+1 . 

0(k) e ft" r(;:) — vector of configuration variables for the k th 
hinge 

p(k) e ft" rCfc) — vector of generalized velocities for the k th 
hinge 



relative spatial velocity for the k' /2 hinge defined as the 
spatial velocity of frame o k with respect to frame Q k 
H*(k) e ft 6xnrCfc) — joint map matrix for the k th hinge, 
whose columns comprise the unit vectors of the hinge. 
We have that A v (k)=H*(k)P(k). 
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vector of (deformation plus hinge) generalized configu- 5 
ration variables for the k lh body 

m= {m 

10 

vector of (deformation plus hinge) generalized veloci- 
ties for the k //z body 


+ U) = k + 1) B(k + l,fc) = 

( o Dm* + a) \ $t3i ik+l)x xr m 
I o <]>(*+ i,jk) J 

the interbody transformation operator which relates 
modal spatial forces and velocities between the and 
(k+l) th bodies 

I o \ 


V W = V (f t) = (^) )e*6. 
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C(U~ 1) = 




£ $ 6 ti #) x 6 


spatial velocity of the k r/l body reference frame F k , 
with to(k) and v(k) denoting the angular and linear 
velocities respectively of frame T k 
V(Ojt) e at 6 — spatial velocity of frame Q k 
V( o k ) e s^ 6 — spatial velocity of frame o k 
V,(j*) e sr 6 — spatial velocity of the j" 1 node on the k th 
body. 

a/j*) 6 Bt 6 — spatial acceleration of the ) th node on the k th 
body. 

«>-(S 

modal spatial velocity of the k //j body 

a w (k)=V w (k) e $ jF(k) — modal spatial acceleration of 
the k" 2 body 

a m (k) e & W (k) — modal Coriolis and centrifugal accel- 
erations for the k' /2 body 

b m (k) e & Jf (k) — modal gyroscopic forces for the k r/z - 
body 

f m (k) e (k) — modal spatial force of interaction 

between the k f/i and (k+l) ,/l bodies 
fy(j*) 6 — spatial force at node j k 

f(k) e s % 6 — effective spatial force at frame T k 
T(k) e at A^(k)-— generalized force for the k //z body 
H jr(k)=H(k)(j)(<p fc , k) e & nt( - ky * 6 —, joint map matrix 
referred to frame p k for the k i/j hinge 

//-[**(*)]* \ m /finJTv,. 

{> yo arm J 

(deformation plus hinge) modal joint map matrix for 
the k' /l body 

A, k) J hstAW 

( ♦(. M ) 

relates spatial forces and velocities between node t k and 
frame T k 

#(k+l,k)=[0, ty(t k+v k)] e $ 6x A/(k) — relates spatial 
forces and velocities between node i k+l and frame T k 


20 B(k)=[<Kk,U c|)(k,2 fc ), . . . , 4>(k,n^.(k))] € 

relates the spatial velocity of frame T k to the spatial 
velocities of all the nodes on the k r/i body when the 
body is regarded as being rigid 

M e sjt — the multibody system mass matrix 

25 C e sfi Af — the vector of Coriolis, centrifugal and elastic 
forces for the multibody system 
2. Introduction 

The invention uses spatial operators ([1, 2]) to formulate 
the dynamics and develop efficient recursive algorithms for 
30 flexible multibody systems. Flexible spacecraft, limber 
space manipulators, and vehicles are important examples of 
flexible multibody systems. Key features of these systems 
are the large number of degrees of freedom and the com- 
plexity of their dynamics models. 

35 Some of the goals of the invention are: (1) providing a 
high-level architectural understanding of the structure of the 
mass matrix and its inverse; (2) showing that the high-level 
expressions can be easily implemented within the very well 
understood Kalman filtering and smoothing architecture; (3) 
40 developing very efficient inverse and forward dynamics 
recursive algorithms; and (4) analyzing the computational 
cost of the new algorithms. Accomplishing these goals adds 
to the rapidly developing body of research in the recursive 
dynamics of flexible multibody systems (see [3, 4, 5]). 

45 It is assumed that the bodies undergo small deformations 
so that a linear model for elasticity can be used. However, 
large articulation at the hinges is allowed. No special 
assumptions are made regarding the geometry of the com- 
ponent bodies. To maximize applicability, the algorithms 
50 developed here use finite-element and/or assumed-mode 
models for body flexibility. For notational simplicity, and 
without any loss in generality, the main focus of this 
specification is on flexible multibody serial chains. Exten- 
sions to tree and closed-chain topologies are discussed. 

55 In Section 3 we derive the equations of motion and 
recursive relationships for the modal velocities, modal 
accelerations, and modal forces. This section also contains a 
derivation of the Newton-Euler Operator Factorization of 
the system mass matrix. A recursive Newton-Euler inverse 
60 dynamics algorithm to compute the vector of generalized 
forces corresponding to a given state and vector of gener- 
alized accelerations is described in Section 4. 

In Section 5, the Newton-Euler factorization of the mass 
matrix is used to develop a partly recursive composite-body 
65 forward dynamics algorithm for computing the generalized 
accelerations of the system. The recursive part is for com- 
puting the multibody system mass matrix. This forward 
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dynamics algorithm is in the vein of well-established 
approaches ([ 6 , 7 ]) which require the explicit computation 
and inversion of the system mass matrix. However, the new 
algorithm is more efficient because the mass matrix is 
computed recursively and because the detailed recursive 
computations follow the high-level architecture (i.e. road- 
map) provided by the Newton-Euler factorization. 

In Section 6 we derive new operator factorization and 
inversion results for the mass matrix that lead to the recur- 
sive articulated-body forward dynamics algorithm. A new 
mass matrix operator factorization, referred to as the Inno- 
vations factorization, is developed. The individual factors in 
the innovations factorization are square and invertible opera- 
tors. This is in contrast to the Newton-Euler factorization in 
which the factors are not square and therefore not invertible. 
The Innovations factorization leads to an operator expres- 
sion for the inverse of the mass matrix. Based on this 
expression, in Section 7 we develop the recursive articulated 
body forward dynamics algorithm for the multibody system. 
This algorithm is an alternative to the composite-body 
forward dynamics algorithm and requires neither the explicit 
formation of the system mass matrix nor its inversion. The 
structure of this recursive algorithm closely resembles those 
found in the domain of Kalman filtering and smoothing 
([ 8 ]). 

In Section 8 we compare the computational costs for the 
two forward dynamics algorithms. It is shown that the 
articulated body forward dynamics algorithm is much more 
efficient than the composite body forward dynamics algo- 
rithm for typical flexible multibody systems. In Section 9 we 
discuss the extensions of the formulation and algorithms in 
this specification to tree and closed-chain topology multi- 
body systems. 

SUMMARY OF THE INVENTION 

A robot manipulator controller for a flexible manipulator 
arm having plural bodies connected at respective movable 
hinges and flexible in plural deformation modes correspond- 
ing to respective modal spatial influence vectors relating 
deformations of plural spaced nodes of respective bodies to 
the plural deformation modes, operates by computing articu- 
lated body quantities for each of the bodies from respective 
modal spatial influence vectors, obtaining specified body 
forces for each of the bodies, and computing modal defor- 
mation accelerations of the nodes and hinge accelerations of 
the hinges from the specified body forces, from the articu- 
lated body quantities and from the modal spatial influence 
vectors. In one embodiment of the invention, the controller 
further operates by comparing the accelerations thus com- 
puted to desired manipulator motion to determine a motion 
discrepancy, and correcting the specified body forces so as 
to reduce the motion discrepancy. 

Computing the articulated body quantities is carried out 
for each body beginning at the outermost body by comput- 
ing a modal mass matrix, computing an articulated body 
inertia from the articulated body inertia of a previous body 
and from the modal mass matrix, computing an articulated 
hinge inertia from the articulated body inertia, computing an 
articulated body to hinge force operator from the articulated 
hinge inertia, computing a null force operator from the 
articulated body to hinge force operator. This is followed by 
revising the articulated body inertia by transforming it by the 
null force operator. 

The manipulator bodies and hinges are characterized by 
respective vectors of deformation and hinge configuration 
variables, and computing modal deformation accelerations 
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and hinge accelerations is carried out for each one of the 
bodies beginning with the outermost body by computing a 
residual body force from a residual body force of a previous 
body and from the vector of deformation and hinge con- 
5 figuration variables, computing a resultant hinge accelera- 
tion from the body force, the residual body force and the 
articulated hinge inertia, and then, for each one of the bodies 
beginning with the innermost body, by computing a modal 
body acceleration from a modal body acceleration of a 
1Q previous body, computing a modal deformation acceleration 
and hinge acceleration from the resulting hinge acceleration 
and from the modal body acceleration transformed by the 
body to hinge force operator. 

Computing a resultant hinge force is followed by revising 
the residual body force by the resultant hinge force trans- 
formed by the body to hinge force operator, and computing 
a modal deformation acceleration and hinge acceleration is 
followed by revising the modal body acceleration based 
upon the deformation and hinge acceleration. The comput- 
ing is performed cyclically in a succession of time steps, and 
20 the vector of deformation and hinge configuration variables 
is computed from the modal deformations and hinge accel- 
erations of a previous time step, or is derived by reading 
robot joint sensors in real time. 

In a preferred embodiment, the articulated body inertia, 
25 the articulated hinge inertia, the body to hinge force opera- 
tor, the null force operator, the body force, the residual body 
force, the resultant hinge acceleration and the resultant hinge 
force are each partitioned into free and rigid versions. This 
embodiment operates by computing the flexible version of 
30 the resultant hinge force from the applied body force, and 
computing the flexible version of the residual body force and 
from the rigid version of the residual body force transformed 
by the modal spatial influence vector. The articulated body 
inertia is decomposed into rigid-free and rigid-rigid coupling 
35 components, and the rigid version of the residual body force 
is revised based upon a function of the rigid-rigid and 
rigid-free coupling components of the articulated body iner- 
tia and a flexible version of the articulated body inertia. This 
embodiment decomposes the manipulator’s modal mass 
4Q matrix into rigid-free and rigid-rigid coupling components 
and computes the rigid-rigid and rigid-free coupling com- 
ponents of the articulated body inertia from respective ones 
of the rigid-rigid and rigid-free coupling components of the 
modal mass matrix. 

45 In this embodiment, free and rigid versions of a defor- 
mation and hinge modal joint map matrix are computed for 
each body so that the flexible version of the articulated hinge 
inertia is computed from the articulated body inertia trans- 
formed by the flexible version of the corresponding defor- 
50 mation and hinge modal joint map matrix, the rigid version 
of the articulated body inertia is computed from a function 
of the rigid-rigid and rigid-free coupling components of the 
articulated body inertia transformed by the flexible version 
of the corresponding deformation and hinge modal joint map 
55 matrix, the rigid version of the articulated hinge inertia is 
computed from the rigid version of the articulated body 
inertia, and the rigid version of the body to hinge force 
operator is computed from the rigid versions of the articu- 
lated body inertia and the articulated hinge inertia. The free 
60 and rigid versions of the deformation and hinge modal joint 
map matrix are formed by computing a joint map matrix 
corresponding to unit vectors of the hinges and computing 
the deformation and hinge modal joint map matrix from the 
joint map matrix and from the modal spatial influence 
65 vector. 

In this embodiment, the flexible version of the resulting 
hinge acceleration is computed from the flexible versions of 
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the articulated hinge inertia and resulting hinge force, and 
the rigid version of the resulting hinge acceleration is 
computed from the rigid versions of the articulated hinge 
inertia and resulting hinge force. The residual body force is 
revised in this embodiment by adding to the residual body 5 
force a product of the rigid versions of the resultant hinge 
force and the body to hinge force operator. 

BRIEF DESCRIPTION OF THE DRAWINGS 

10 

FIG. la is a simplified diagram of a portion of a robot 
manipulator having flexible links, and illustrating the coor- 
dinate system employed in one embodiment of the inven- 
tion. 

FIG. lb is a simplified diagram illustrating the finite 15 
element analysis employed in the invention, in which the 
displacement of plural spaced nodes along the length of a 
flexible link follows a well-recognized pattern for each 
mode of flexibility. 

20 

FIG. 2 is a block diagram illustrating how the articulated 
body quantities are produced in one embodiment of the 
invention. 

FIG. 3 is a block diagram illustrating an articulated body 
forward dynamics algorithm for flexible link manipulators in 25 
accordance with the present invention. 

FIG. 4 is a block diagram illustrating the process of the 
invention for controlling a robot manipulator having flexible 
links. 

FIGS. Sa and 5b constitute a block diagram illustrating a 30 
preferred embodiment of the articulate body forward 
dynamics algorithm employed in the process of FIG. 4. 

FIG. 6 is a simplified schematic block diagram of appa- 
ratus embodying the present invention. 

35 

DETAILED DESCRIPTION OF THE INVENTION 

3. Equations of Motion for Flexible Serial Chains 

In this section, we develop the equations of motion for a 
serial flexible multibody system with N flexible bodies. 40 
Each flexible body is assumed to have a lumped mass model 
consisting of a collection of nodal rigid bodies. Such models 
are typically developed using standard finite element struc- 
tural analysis software. The number of nodes on the k^ body 
is denoted n^Ck). The ] th node on the k r/l body is referred to 45 
as the ] k th node. Each body has associated with it a body 
reference frame, denote p k for the k* 71 body. The deforma- 
tions of the nodes on the body are described with respect to 
this body reference frame, while the rigid body motion of the 
k f/l body is characterized by the motion of frame T k . 

The 6-dimensional spatial deformation (slope plus trans- 
lational) of node j fc (with respect to frame f k ) is denoted 
u(j*) € gt G . The overall deformation field for the k th body is 
defined as the vector u(k)=col{u(y} e The vector 55 

from frame T k to the reference frame on node } k is denoted 

With M^k*) e $ 6x6 denoting the spatial inertia of the 
] ih node, the structural mass matrix for the k lh body M 5 (k) 
is the block diagonal matrix diagjM/j*)} e $ 6 " s( * )x6 ' is( * ) . 60 
The structural stiffness matrix is denoted K 5 (k) e $ 6 " s( * )x 
e n s( *\ Both M/k) and K^(k) are typically generated using 
finite element analysis. 

As shown in FIG. la, the bodies in the serial chain are 
numbered in increasing order from tip to base. We use the 65 
terminology inboard (outboard) to denote the direction along 
the serial chain towards (away from) the base body. The k r7z 
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body is attached on the inboard side to the (k+1)** body via 
the k th hinge, and on the outboard side to the (k-1)^ body 
via the (k-l) r/z hinge. On the k th body, the node to which the 
outboard hinge (the (k-l)' 7 * hinge) is attached is referred to 
as node t k , while the node to which the inboard hinge (the k ih 
hinge) is attached is denoted node d*. Thus the k ih hinge 
couples together nodes d* and l k+1 . Attached to each of these 
pair of adjoining nodes are the k th hinge reference frames 
denoted O k and o k , respectively. The number of degrees 
of freedom for the k /;i hinge is denoted n r (k). The vector of 
configuration variables for the k /A hinge is denoted 0(k) 
e $" rCA:) , while its vector of generalized speeds is denoted 
P(k) e ^ nrW . In general, when there are nonholonomic hinge 
constraints, the dimensionality of p(k) may be less than that 
of 0(k). For notational convenience, and without any loss 
generality, it is assumed here that the dimensions of the 
vectors 0(k) and p(k) are equal. In most situations, (3(k) is 
simply 0. However there are many cases where the use of 
quasi-coordinates simplifies the dynamical equations of 
motion and an alternative choice for (3(k) may be preferable. 
The relative spatial velocity A v (k) across the hinge is given 
by H*(k)P(k), where H*(k) denotes the joint map matrix for 
the k th hinge. 

Assumed modes are typically used to represent the defor- 
mation of flexible bodies, and there is a large body of 
literature dealing with their proper selection. There is how- 
ever a close relationship between the choice of a body 
reference frame and the type of assumed modes. The com- 
plete motion of the flexible body is contained in the knowl- 
edge of the motion of the body reference frame and the 
deformation of the body as seen from this body frame. In the 
multibody context, it is often convenient to choose the 
location of the k' /l body reference frame j^^as a material 
point on the body and fixed to node d* at the inboard hinge. 
For this choice, the assumed modes are cantilever modes and 
node d k exhibits zero deformation (u(d^)=0). Free-free 
modes are also used for representing body deformation and 
are often preferred for control analysis and design. For these 
modes, the reference frame ? k is not fixed to any node, but 
is rather assumed to be fixed to the undeformed body, and as 
a result all nodes exhibit nonzero deformation. The dynam- 
ics modeling and algorithms developed here handle both 
types of modes, with some additional computational sim- 
plifications arising from Eq. (1) when cantilever modes are 
used. For a related discussion regarding the choice of 
reference frame and modal representations for a flexible 
body see [9]. 

We assume here that a set of n m (k) assumed modes has 
been chosen for the k ,/z body. Let Il/(k) e denote the 
modal spatial displacement vector at the ) k h node for the r th 
mode. The modal spatial displacement influence vector IF(k) 
e for the ] k h node and the modal matrix H(k) 

e ^ 6ns( * )x ” mW for the k th body are defined as follows: 

IP(k)=l=n/(k), . . . II„ w(Jt /(k)] and II(k)=col{IF(k)} 

The r* h column of H(k) is denoted H r (k) and defines the 
mode shape for the I th assumed mode for the k th body. Note 
that for cantilever modes we have 

Il/(k)=0 for r=l ... n m (k) (1) 

With T](k) e denoting the vector of modal deformation 
variables for the k r/l body, the spatial deformation of node j fc 
and the spatial deformation field u(k) for the k' /z body are 
given by 


u(j A .)=lf(k)Ti(k) and u(k)=II(k)n(k) 


( 2 ) 



5,546,508 


The vector of generalized configuration variables v(k) and 
generalized speeds X(k) for the k! h body are defined as 


V ec *3 ) 


l m ) 


where .V(k) An m (k)+n r (k). The overall vectors of general- 
ized configuration variables v and generalized speeds X for 
the serial multibody system are given by 


v £ col{v(k)} e % Jsf and X a col{X(k)} e M 


V(O k )=V{O k + )+H*(k)m (8) 

The spatial velocity V(k) of the k" 1 body reference frame is 
given by 

V(k) =if*( 0 kJc)V( 0 k)- U (4) P) 

=r< & k,m 0 k)-^(mk) 

Putting together Eq. (5), Eq. (7), Eq. (8) and Eq. (3.1), it 


( 4 ) 10 follows that 




denotes the overall number of degrees of freedom for the 
multibody system. The state of the multibody system is 
defined by the pair of vectors {v,X}. For a given system state 2Q 
{v,X}, the equations of motion define the relationship 
between the vector of generalized accelerations X and the 
vector of generalized forces T e ^ j\f for the system. The 
inverse dynamics problem consists of computing the vector 
of generalized forces T for a prescribed set of generalized 25 
accelerations X. The forward dynamics problem is the 
converse one and consists of computing the set of general- 
ized accelerations X resulting from a set of generalized 
forces T. The equations of motion for the system are 
developed in the remained of this section. 30 

3.1 Recursive Propagation of Velocities 
Let V(k) e % 6 denote the spatial velocity of the k th body 
reference frame T k . The spatial velocity V,(t fc+1 ) e 6 of 
node t k+1 (on the inboard of the k th hinge) is related to the 
spatial velocity V(k+1) of the (k+l) f/z body reference frame 35 
F k+V and the modal deformation variable rates r|(k+l) as 
follows: 

v,(t i+ i) = p(k + l,t M )V(k + 1) + U ( 1 M ) (5) 40 

= «/*(k + Uk*i)V(k + 1) + jt \k + l)r|(i + 1) 

The spatial transformation operator <J>(x,y) e $ 6x6 above is 
defined to be 


■dr ) 


V(k)=<\>* (k + 1 ,k) v(k+ 1 M*(k k+1 , k)ir(k+i)T)(k+i)-Hb*( o h 

k)H*(k)${k)-II d (k)r\{k) (10) 

Thus, with Ar(k) An m (k)+6, and using Eq. (10), the modal 
spatial velocity V m (k) e gt V (k) for the k th body is given by 

vjk) =£= ^ ) = 0*(t + U)V m (i+l)+ (11> 

where the interbody transformation operator $(.,.) and the 
modal joint map matrix ft(k) are defined as 

®(iH-U)^( 0[,l ' (i + 1)] *‘ , ’ ( '* +, ’ t) <12> 

l,) lo <Kt+i,i) r 

«(i)A ( 1 - [n ^* \ e a MikyJTik) 

W 0 Hf(k) 


H jF(k) AH(k)(J)( O k , k) 6 Sft"KA)x6 
Note that 

0(*+U)=.4(&+l) B(k+ljc) 


■AfHAl [K ‘ m * 


where l(x,y) e denotes the vector between the points x 
and y. Note that the following important (group) property 
holds: 

50 

0(x,y)<Ky,z)=4(x>z) 

for arbitrary points x, y and z. As in Eq. (5), and throughout 
this specification, the index k will be used to refer to both the 
k ih body as well as to the k lh body reference frame ? k with 55 
the specific usage being evident from the context. Thus for 
instance, V(k) and (j)(k,t^) are the same as V( f k ), and <()( 
t fc ) respectively. 

The spatial velocity V( o k ) of frame O k (on the inboard 6Q 
side of the k lh hinge) is related to V^(L +1 ) via 

V(O k + )=$*(t k+v O k )V s (t k+ 1) (7) 

Since the relative spatial velocity A v (k) across the k" 1 hinge 65 
is given by H*(k)p(k), the spatial velocity VfoJ of frame 
O k on the outboard side of the k' /z hinge is 


B(k + U)*[0, W k+1 ,k) U 9t 6x (k) 

Also, the modal joint map matrix w(k) can be partitioned as 

««=( (16) 

where 

n/ k)A[I-[lE(k)P] g and W r (k)*[0, H(k)<K O h 

k)]e st "'<*>* 0 ( *> (17) 


* = s *(k). 


we define the spatial operator s<& as 


0(2,1) o 

0 0(3,2) 


. O (N,N- 1) 0 j 
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Using the fact that £ 0 is nilpotent (i.e. £<^=0), we define 
the spatial operator 3> as 

a>* [i-e^r 1 = 09) 


/ + €& + . • • + eX 1 = 


*(2,1) 


0 

1 


. . 0 

. . 0 


\ 


6 st WxW 


®(M 2) . . . Ij 


where 


5 


10 


15 


0(i,y) £0(U— 1) . . - for i>j 


Also define the spatial operator n & diag { ft(k)} e $ jvfx W. 
Using these spatial operators, and defining V m a col{V m (k)} 2Q 

e from Eq. (11) it follows that the spatial operator 
expression for V m is given by 


V m -<D* 7i*X (20) 

25 

3.2 Modal Mass Matrix for a Single Body 
With V s Q k ) e sr 6 denoting the spatial velocity of node j*, 
and V,.(k) aco1{V^(j^)} e ^ 6nsW the vector of all nodal spatial 
velocities for the k tfl body, it follows (see Eq. (5)) that 

30 

VJto=B*{k w*>+ a (kMim, B*(k))VJk) (21) 

where 

B(k) 4[<|>(k f l*) f <Kk,2*), . . . <Kk,n 5 (k))] e (22) 35 
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in this case n m (k)-0 (and II(k) is null). 

Since the vector l(k,j*) from T k to node j* depends on 
the deformation of the node, the operator B(k) is also 
deformation dependent. From Eq. (23) it follows that while 
the block k) is deformation independent, both the 
blocks M m /r (k) and M m rr (k) are deformation dependent. The 
detailed expression for the modal mass matrix can be 
defined using modal integrals which are computed as a part 
of the finite-element structural analysis of the flexible bod- 
ies. These expressions for the modal integrals and the modal 
mass matrix of the k' ; * body can be found in [10]. Often the 
deformation dependent parts of the modal mass matrix are 
ignored, and free-free eigen-modes are used for the assumed 
modes H(k). When this is the case, M m /r (k) is zero and 
M„/(k) is block diagonal. 

3.3 Recursive Propagation of Accelerations 
Differentiating the velocity recursive equation, Eq. (11), 
we obtain the following recursive expression for the modal 

spatial acceleration a m (k) e $ W (k)for the k th body: 


• / nw \ ( ) 

M*) ±V m (*)= = 

y a(k) J 

mk + m^k + 1) + n*(kym + a) 

where a(k)=V(k), and the Coriolis and centrifugal accelera- 
tion term a m (k) e & Jf (k)is given by 


c M k ) = 


d^*(k+hk) 

dt 


V m (k+ 1) + 


d H *(k) 

dt 


X(*> 


(25) 


The detailed expressions for a m (k) can be found in [10]. 

Defining a m =col{a OT (k)} e & AT and arn m ^col{oc m (k)} e & 77, 
and using spatial operators we can rexpress Eq. (24) in the 
form 


Since M 5 (k) is the structural mass matrix of the k th body, and 
using Eq. (21), the kinetic energy of the k th body can be 
written in the form 

V2V*(k)M s (k)V s (k) = ViV m *(k)M m (k)V m (k) (23) 

where 

AUi) A ( ll *W ) M s (k)\U(k), B*(k)\ = 


a m =4>*( H*X+a m ) (26) 

The vector of spatial accelerations of all the nodes for the k^ 
40 body, Oy(k) Acol{a/j*)} e is obtained by differenti- 

ating Eq. (21): 

a s (k)=V s {k)=[II(k), B*(k)}ajk)+a(k) (27) 

where 

45 


( n*(k)M s (k)U{k) U*(k)M s (k)B *(k) 

B(k)M s {k)U(k) B(k)M s (k)B*(k) 

M&k) «*) I 

Corresponding to the generalized speeds vector X(k), M m (k) 
as defined above is the modal mass matrix of the k* ;i body. 55 
In the block partitioning in Eq. (23), the superscripts f and 
r denote the flexible and rigid blocks respectively. Thus 
M n f(k) represents the flex/flex coupling block, white 
M m /r (k) the flex/rigid coupling block of M m (k). We will use 
this notational convention through this specification. This 60 
partitioning is readily carried out by simply recognizing that 
the M m ^(k) block is a square matrix of dimensionality equal 
to the number of deformation modes while the M m rr (k) 
block is a square 6-by-6 matrix. Note that M m rr (k) is 
precisely the rigid body spatial inertia of the k th body. 65 
Indeed, M m (k) reduces to the rigid body spatial inertia when 
the body flexibility is ignored, i.e., no modes are used, since 



a{k) = col{a(jk)} = 


d\m),B*m 

dt 


V m (k) e 


(28) 


3.4 Recursive Propagation of Forces 
Let f(k-l) e $ 6 denote the effective spatial force of 
interaction, referred to frame f k _i, between the k' /z and 
(k-iy* bodies across the (k-l)'* hinge. Recall that the 
(k-l) //z hinge is between node t k on the k /71 body and node 
d^_! on the (k-\) th body. With f 5 (j*)e denoting the spatial 
force at a node j*, the force balance equation for node t k is 
given by 


( 29 ) 


For all nodes other than node t k on the k th body, the force 
balance equation is of the form 


F^=M s (J k )a s Q^b(j k WM ( 30 ) 

In the above f^k)-K v (k)u(k) e denotes the vector of 

spatial elastic strain forces for the nodes on the k //2 body, 
while b(y e sr 6 denotes the spatial gyroscopic force for the 
node ] k and is given by 
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b(k) J \ c * (31) 

where to(j*) e g{ 3 notes the angular velocity of node j*. 
Collecting together the above equations and defining 


/ 0 V 


QKk- 1) = 






and 


(32) 


10 


15 


b(k) £ col{b(j k )} £ s« 6as(k) 

it follows from Eq. (29) and Eq. (30) that 

/ s (Jfc)=C(fe,JM)y(*-l)4^ (33) 20 

where f/k) AcoljfjQ*)} e s^ 6 "^. Noting that 

f Ck>=B (k)f^(k) (34) 

25 

and using the principle of virtual work, it follows from Eq. 

(21) that the modal spatial forces f m (K) e sjt77(k)for the 
k" 1 body are given by 


f (k) a ( mk) 

\ B(k) 


)/^)=( n * (i P ) 


m 


Premultiplying Eq. (33) by 


(35) 30 



35 


and using Eq. (23), Eq. (27), and Eq. (35) leads to the 
following recursive relationship for the modal spatial forces: 


» - (2E 

b m {k)+K m (k)m 

j WtJc - \)flk - 1) + M m (k)a m (k ) + 45 

b m (k)+K m (k)m 

= O (k,k - 1 )f m (k - 1) + M m (k)a m (k) + 

b m (k) + K m (k)m 50 

Here we have defined 


/ mm 
y wk,t k ) 


bnik) = ( U *{® ) [b(k) + M s (k)a(k)] «■ * 77 m 

and the modal stiffness matrix 


(37) 


55 


K m (k) 


n mm) (k) o 
0 0 


Si JT (fc)x Jf {k) 


(38) 


The expression for K m (k) in Eq. (38) uses the fact that the 60 
columns of B*(k) are indeed the deformation dependent 
rigid body modes for the k th body and hence they do not 
contribute to its elastic strain energy. Indeed, when a defor- 
mation dependent structural stiffness matrix K v (k) is used, 
we have that 65 


16 

However the common practice (also followed here) of using 
a constant, deformation-independent structural stiffness 
matrix leads to the anomalous situation wherein Eq. (39) 
does not hold exactly. We ignore these fictitious extra terms 
on the left-hand side of Eq. (39). 

The velocity-dependent bias term b m (k) is formed using 
modal integrals generated by standard finite-element pro- 
grams, and a detailed expression for it is given in [10). From 
Eq. (36), the operator expression for the modal spatial forces 

f m £Col{f m (k)} e sr W for all the bodies in the chain is given 
by 


f m =0(M m a m +b m +K m v) 


(40) 


where 

M^AdiagtM^Ck)} K m Mag{K m (k)} e Si 

and b m £col{b m (k)} 

From the principle of virtual work, the generalized forces 
vector T e $ N for the multibody system is given by the 
expression 

T= Hf m (41) 


3.5 Operator Expression for the System Mass Matrix 
Collecting together the operator expressions in Eq. (20), 
Eq. (26), Eq. (40) and Eq. (41) we have: 


V m = <D* H (*2) 

a m + 

f m = 0(M m a m + b m + K m Q) = 0M m 0*n*t + 


0(M m 0*a m + b m + K m v) 

T = n f m = + 

= M X + C 

where 

M A HO M m O* H* e Si Afx -Afand C a HO(M m O\+b ffl + 
K m v) £ .Af (43) 

Here M is the system mass matrix for the serial chain and 
the expression n* is referred to as the Newton- 

Euler Operator Factorization of the mass matrix. C is the 
vector of Coriolis, centrifugal, and elastic forces for the 
system. 

It is noteworthy that the operator expressions for M and 
C are identical in form to those for rigid multibody systems 
(see [1, 11]). Indeed, the similarity is more than superficial, 
and the key properties of the spatial operators that are used 
in the analysis and algorithm development for rigid multi- 
body systems also hold for the spatial operators defined here. 
As a consequence, a large part of the analysis and algorithms 
for rigid multibody systems can be easily carried over and 
applied to flexible multibody systems. This is the approach 
adopted here. 

4. Inverse Dynamics Algorithm 

This section describes a recursive Newton-Euler inverse 
dynamics algorithm for computing the generalized forces T, 
for a given set of generalized accelerations X and system 
state {v, X}. The inverse dynamics algorithm also forms a 
part of forward dynamics algorithms such as those based 
upon composite body inertias or the conjugate gradient 
method ([12]). 

Collecting together the recursive equations in Eq. (11), 
Eq. (24), Eq. (36) and Eq. (41) we obtain the following 
recursive Newton-Euler inverse dynamics algorithm: 


Kj(k)B*(k)=0 


(39) 
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V m (N + 1) = 0, a m (N + 1) = 0 
for it = W ... 1 

V„(k) = <t*(i + 1 ,k)V m (k + 1) + H *(k)x(k) 
a m (k) = 0 *(k + l,k)a m (k + 1) + n *(k)x(k) + Uk) 
end loop 

/ m (0) = 0 
for ft = 1 ... AT 

Uk) = ®(k,k - 1 )f m (k - 1) + M m (k)a m (k) + b m {k) + K m (kMk) 
T(k)= n (k)f m (k) 
end loop 

15 

The structure of this algorithm closely resembles the recur- 
sive Ncwton-Euler inverse dynamics algorithm for rigid 
multibody systems (see [13, 1]). All external forces on the 
k th body are handled by absorbing them into the gyroscopic 2Q 
force term b m (k). Base mobility is handled by attaching an 
additional 6 degrees of freedom hinge between the mobile 
base and an inertial frame. 

By taking advantage of the special structure of 0(k+l,k) 
and ?i(k) in Eq. (12) and Eq. (13), the Newton-Euler 2 s 
recursions in Eq. (44) can be further simplified. Using block 
partitioning and the superscripts f and r as before to denote 
the flexible and rigid components or versions of the various 
quantities, we have that 



It is easy to verify that Eq. (45) below is a simplified 
version of the inverse dynamics algorithm in Eq. (44). 


(44) 


the generalized forces vector T(k) corresponding to the 
hinge actuator forces T r (k) can be set, white the remaining 
generalized forces T f (k) are zero. Thus in contrast with rigid 
multibody systems, flexible multibody systems are under- 
actuated systems ([14]), since the number of available 
actuators is less than the number of motion degrees of 
freedom in the system. For such under-actuated systems, the 
inverse dynamics computations for the generalized force T 
are meaningful only when the prescribed generalized accel- 
erations X form a consistent data set. For a consistent set of 
generalized accelerations, the inverse dynamics computa- 
tions will lead to a generalized force vector T such that 
T'C)=0. 

5. Composite Body Forward Dynamics Algorithm 
The forward dynamics problem for a multibody system 
requires computing the generalized accelerations X for a 
given vector of generalized forces T and state of the system 
{v,X}. The composite body forward dynamics algorithm 
described below consists of the followings steps: (a) com- 
puting the system mass matrix M, (b) computing the bias 
vector c, and (c) numerically solving the following linear 
matrix equation for X: 


V m (N+ 1) = 0, ct m (N + 1) = 0 
for A: = N ... 1 
VM = r\(k) 

V m ’(k) = r«M,k) A *(k + l)V m (k + 1) + Hi*(k)m ~ d (k)T\(k) 
aj(k)^r[(k) 

a m '(k) = raw ,k) A *(k + l)a m (k + 1 ) + Hi*(k) | 3 (« - n'WnW + a m '(k) 
end loop 

/«( 0) = 0 
for it = 1 ... iV 

uk) = A m(>k,k - 1 )u(k - 1 ) + M m (k)a m (k) + b m (k) + Kjmk) 

= (nk) \ / mm - mm*u(k) 

\r(k) ) ( Hi(k)U(k) 

end loop 


(45) 


In the foregoing algorithm, T|(k) and r|(k) are the modal 
deformation velocities and accelerations, respectively, com- 
puted from the results obtained for a previous time step by 
a forward dynamics algorithm of the type described below 65 
herein. Flexible multibody systems have actuators typically 
only at the hinges. Thus for the k‘ h body, only the subset of 


MX=T~ C (46) 

Later in Section 6 we describe the recursive articulated body 
forward dynamics algorithm that does not require the 
explicit computation of either M or c. 
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It is evident from Eq. (46) that the components of the 
vector c are the generalized forces for the system when the 
generalized accelerations X are all zero. Thus C can be 
computed using the inverse dynamics algorithm in Eq. (45). 

We describe next an efficient composite-body-based recur- 5 
sive algorithm for the computation of the mass matrix M . 
This algorithm is based upon the following lemma which 
contains a decomposition of the mass matrix into block 
diagonal, block upper triangular and block lower triangular 10 
components. 

Lemma 5.1 

Define the composite body inertias R(k) e (k)x 
^(k)recursively for all the bodies in the serial chain as 
follows: 15 

r /?(0) = 0 (47) 

for fc = 1 . . . iV 

R{ k) = 0(U - 1 )R(k - 1)©*(U - 1) + M m (k) 20 

end loop 

Also define RAdiag{R(k)} e Then we have the 

following spatial operator decomposition 25 

(48) 

where OaO-I: 30 

Proof: See Appendix A. 

Physically, R(k) is the modal mass matrix of the compos- 
ite body formed from all the bodies outboard of the k th hinge 
by freezing all their (deformation plus hinge) degrees of 
freedom. It follows from Eq. (43) and Lemma 5.1 that 35 

M= 7i H *= HR H *+ 7{RQ>R H *+ KR®* H * (49) 

40 

Note that the three terms on the right of Eq. (49) are block 
diagonal, block lower triangular and block upper triangular 
respectively. The following algorithm for computing the 
mass matrix M computes the elements of these terms 
recursively. 


20 

element is computed, a new recursion to compute the 
off-diagonal elements is spawned. The structure of this 
algorithm closely resembles the composite rigid body algo- 
rithm for computing the mass matrix of rigid multibody 
systems ([12, 8]). Like the latter, it is also highly efficient. 
Additional computational simplifications of the algorithm 
arising from the sparsity of both H/X) and w r (k) are easy 
to incorporate. 

6. Factorization and Inversion of the Mass Matrix 

An operator factorization of the system mass matrix M, 
denoted the Innovations Operator Factorization, is derived 
in this section. This factorization is an alternative to the 
Newton-Euler factorization in Eq. (43) and, in contrast with 
the latter, the factors in the Innovations factorizations are 
square and invertible. Operator expressions for the inverse 
of these factors are developed and these immediately lead to 
an operator expression for the inverse of the mass matrix. 
The operator factorization and inversion results here closely 
resemble the corresponding results for rigid multibody sys- 
tems (see [1]). 

Given below is a recursive algorithm illustrated in FIG. 2 
which defines some required articulated body quantities. In 
the following algorithm, P(k) is the articulated body inertia 
of body k, D(k) is the articulated hinge inertia of hinge k, 
G(k) is a body to hinge force operator of body and hinge k, 
and T(k) is a null force operator for hinge k which accounts 
for the component of applied force resulting in no hinge 
acceleration. 


for k=l ... N 

R(k) = ®(k,k-l)R(k-m*(k,k-l) + M m (k) 

= \k)${t k ,k - \)R rr {k - 1)4 )*(t k ,k - 1) A *(k) + M m {k) 

X(k) = R(k) n *(k) 

= H w(k) 

for/= (fc + 1) . . . N 

X(j) = ®(M-1)X(/-U= 

M (j.k) = M *(k,h)= H (j)X(j) 

end loop 

^ end loop 

The main recursive proceeds from tip to base, and computes 
the blocks along the diagonal of M. As each such diagonal 
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TO) 

fork- 1 . . .N 

= 0 


TO 

= 

0(U- l)/*(i t- 1)4>*(W” 1 ) + M m i 

D(k) 

= 

n (k)P(k) H *(k) e ® 77 (k)xJ 7 o fc ) 

G(k) 

= 

TO ^*(k)D~ l (k) z& 77 mMik) 

K(k + U) 

= 

<P(k+Uk)G(k)e ®77 (fc)xW(A) 

X(k) 

= 

/ - G(k) H (k ) e St 77 (k)xJ 7 m 

TO) 

= 

x(l k)P(k)E ^77 (k )*77 (k) 

\\f(k+ l,k) 

= 

0 (k + lk)x(k)E ^77 (k )*77 (*) 


77 (k)x 77 (k) 


end loop 


The operator P e $ 77x 77 is defined as a block diagonal 
matrix with the k £/l diagonal element being P(k). The quan- 
tities defined in Eq. (51) form the component elements of the 2Q 
following spatial operators: 


Lemma 6.2 

[/+ 7 i\\fK] 


(55) 


D & HPH*=diag{D(k)} e $77*77 
G &P H *£T 1 =diag{G(fc)} e $77*77 

K ^Ge $ 77 x J7 

i^/-G?i=diag{t(^)} e $ 77*77 

e $77x77 (52) 


The only nonzero block elements of K and are the 30 
elements’ K(k+l,k)’s and \j/(k+l,k)’s respectively along the 
first sub-diagonal. 

As in the case for s<t>, e w is nilpotent, so we can define 
the operator \j / as follows. 


// o 
¥( 2 , 1 ) / 


v a (/- c ¥ r 


o\ 

0 


VWl) V(M2) 


(53) 


35 


: $ 77 x 77 


40 


where 

& y(i,i - 1) . . . \|/(/ + 1 J) for 7li > j 


The structure of the operators e w and \j / is identical to that 45 
of the operators s® and <f> respectively except that the 
component elements are now \|/(i,j) rather than <D(i,j). Also, 
the elements of \j/ have the same semigroup properties as the 
elements of the operator O, and as a consequence, high-level 
operator expressions involving them can be directly mapped 50 
into recursive algorithms, and the explicit computation of 
the elements of the operator is not required. 

The Innovations Operator Factorization of the mass 
matrix is defined in the following lemma. 55 

Lemma 6.1 J ‘ 


Proof: See Appendix A. 

It follows from Lemma 6.1 and 6.2 that the operator expres- 
sion for the inverse of the mass matrix is given by: 
Lemma 6.3 


TiMfK] *zr 1 [/- HyK\ (56) 

Once again, note that the factor [I- is square, block 
lower triangular and nonsingular and so Lemma 6.3 pro- 
vides a closed-form expression for the block LDL* decom- 
position of AT 1 . 

7. Articulated Body Forward Dynamics Algorithm 

We first use the operator expression for the mass matrix 
inverse developed in Section 6 to obtain an operator expres- 
sion for the generalized accelerations X. This expression 
directly leads to a recursive algorithm for the forward 
dynamics of the systems. The structure of this algorithm is 
completely identical in form to the articulated body algo- 
rithm for serial rigid multibody systems. The computational 
cost of this algorithm is further reduced by separately 
processing the flexible and hinge degrees of freedom at each 
step in the recursion, and this leads to the articulated body 
forward dynamics algorithm for serial flexible multibody 
systems. This algorithm is an alternative to the composite- 
body forward dynamics algorithm developed earlier. 

The following lemma describes the operator expression 
for the generalized accelerations X in terms of the general- 
ized forces T. 

Lemma 7.1 

Ml- Hym *D~ 1 [T— 7i y/{ KT+P a m +b m +K m v } ]-K*vf *a m (57) 


AH/+ H<J>K)D[I+ K&K] * (54) 

Proof: See Appendix A. 

Note that the factor [I+wd>K] e $77X77 is square, 
block lower triangular and nonsingular, while D is a block 
diagonal matrix. This factorization provides a closed-form 
expression for the block LDL* decomposition of At. The 65 
following lemma gives the closed form operator expression 
for the inverse of the factor [1+ hOK]. 


Proof: 

See Appendix A. 

As in the case of rigid multibody systems ([1, 2]), the 
direct recursive implementation of Eq. (57) leads to the 
following recursive forward dynamics algorithm illustrated 
in FIG. 3. In the following algorithm, z(k) is a residual body 
force on body k, e(k) is the resultant hinge force on hinge k, 
v(k) is the resultant hinge acceleration of hinge k and z + (k) 
is the revised residual body force on body k: 


60 



23 


5,546,508 


24 


z + ( 0) = 0 


for k = 1 . . . n 


z(k) 

0(U ~ 1 )z + (k - 1) + P(k)a m (k) + 


b m {k) + K m (k)m 

e(fc) = 

T(k) — H {k)z{k) 

v(*) = 

D-HkMk) 

z + (k) = 

z(k) + G(kUk) 

end loop 


a m (n + 1) = 0 


for k = n . . . 1 


&m + (k) = 

®*(k+l,k)a m (k+l) 

X • 

v{k)~G*(k)a m +(k) 

a m (k) 

0 Cm + (k) + ^*(k)x(k) + CCm(k) 


end loop 


ever each sweep in the algorithm now contains twice as 
many steps as the original algorithm. But since each step 
now processes only a smaller number of degrees of freedom, 
this leads to a reduction in the overall cost. In the following 
5 algorithm, the subscript r denotes the rigid component or 
version of the subscripted quantity while the subscript f 
denotes the flexible component or version of the subscripted 
quantity. Thus, n/k) is a matrix including the correspond- 
10 ing modal spatial influence vector, while ft r (k) is a matrix 
including the corresponding transformed joint map matrix. 
The new algorithm (replacing Eq. (51) for computing the 
articulated body quantities is as follows: 


15 


20 


P + (0) 

for & = 1 ... AT 

m 

m 


(62) 


0 


B (lc, k- l)P+(k-l) B *(k, k- 1) g ^ 6x6 
A (k)T(k) -A*(k) + M m (k) e StTTikyxTT (*) 


The structure of this algorithm is closely related to the 
structure of the well known Kalman filtering and smoothing 
algorithms ([8]). All the degrees of freedom for each body 
(as characterized by its joint map matrix w*(.)) are pro- 25 
cessed together at each recursion step in this algorithm. 
However, by taking advantage of the sparsity and special 
structure of the joint map matrix, additional reduction in 
computational cost is obtained by processing the flexible 
degrees of freedom and the hinge degrees of freedom 
separately. These simplifications are described in the fol- 30 
lowing sections. 

7. 1 Simplified Algorithm for the Articulated Body Quan- 
tities 

Instead of a detailed derivation, we describe here the 
conceptual basis for the separation of the modal and hinge 35 
degrees of freedom for each body. First we recall the 
velocity recursion equation in Eq. (11) 

V m (k)=&*(k+1 ,k)V m (k+ 1 )+ ?i*(k)X(k) 

and the partitioned form of n( k) in Eq. (13) 

*«>-( Z) 

Introducing a dummy variable k', we can rewrite Eq. (59) as 
V m (k')=<S>*(k+l,k r )V m (k+\)+ H*j(k)r\(k) 


45 

(61) 


(59) 

40 

(60) 


Dj{k ) 


n Ak)P(k) n j*(k ) 6 

Gj(k) 

= 

P(k) n f(k)DfKk) e ® ^ (W) 

^k) 

= 

k 

& 

k 

& 

UJ 

s 

1 

1 

Ptf) 

= 

Xj(k)P(.k)e^^ 

Drik) 

= 

n Ak)PAk) n r*(k) e ^rik^rik) 

G r (k) 

= 

P r (k) n *(k)D;\k) e & ^ »>«|<*) 

T r (k) 

= 

/ - Grik) n r (k) e ^ (ft* 77 (*) 

P+(k) 

= 

TfflPffle MAT mJVto 

1 ,k) 

= 

0>(* + l,*)T(*)e a ^ W 


^ end loop 

We now use the sparsity of £(k+l,k), n/k) and H r { k) to 
further simplify the above algorithm. Using the symbol “x” 
to indicate “don’t care” blocks, the structure in block par- 
titioned form of some of the quantities in Eq. (62) is given 
below. In the following algorithm, the subscripts f and r have 
the same significance as that discussed previously herein, 
the subscript R denotes another rigid version of the sub- 
scripted quantity (defined below), while P r/ (k) and P rr (k) 
denote the blocks of the articulated body inertia P(k) parti- 
tioned in the same manner as that discussed previously 
herein with reference to the partitioning of the modal mass 
matrix in Equation (23): 


where 


<D(k+ 1 ,k') £<&(k+ 1 ,k) and ®(k\k)£I 


m = <k* a , k - 1 )p R + (k - i)(j >*(**, k - 1), 
50 

(P R + (k) is defined below) 


Conceptually, each flexible body is now associated with two 
new bodies. The first one has the same kinematical and 
mass/inertia properties as the real body and is associated 55 
with the flexible degrees of freedom. The second body is a 
fictitious body and is massless and has zero extent. It is 
associated with the hinge degrees of freedom. The serial 
chain now contains twice the number of bodies as the 
original one, with half the new bodies being fictitious ones. 60 
The new h* operator now has the same number of columns 
but twice the number of rows as the original n* operator. 
The new operator has twice as many rows and columns as 
the original one. Repeating the analysis described in the 
previous sections, we once again obtain the same operator 65 
expression as Eq. (57). This expression also leads to a 
recursive forward dynamics algorithm as in Eq. (58). How- 


Gj(k) = ^ ^ , where g(k) - \i(k)D^(k) 

and p (k) £ [P rf (k), P^Qc)} H?{k) edt 6xnmQi) 


P rik) - 


X X 
X Pr(Jc) 


, where P R (k) = P rr (k ) - g(fc)|i*(*) £ ^ 6x6 
D r (k ) = // T 7 (k)P R (k)H T *(fc) e^r^xnKk) 

G T {k) = ( G * {k) ) , where G R (k) ± P R Qc)H^{k)D~\k) 

| , where % R (k) = I - G R {k)HHk) e^ 6x6 


T r (fc) = 


I X 

o t m 
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-continued 





, where P R + {k) = x R (k)P R {k) e^ 6x6 


Using the structure described above the simplified algorithm 
for computing the articulated body quantities is as follows: 


(63) 


for k = 1 . . 

.N 



m 

= 



P(k) 

= 



DM ) 

= 

H j(k)P(k) H f(k) 

15 

m 

= 

[P'Kk), P"(k)\ H J*(k ) 


8(k) 

= 

mD~/(k) 


PR(k) 

= 

P"(k) - g(k)ji*(k) 

20 

D R (k) 

= 

Hi(k)P R (k)Hi*(k) 

G R (k) 

= 

P R (k)Hi*(k)D- R \k) 


?/?(*) 

= 

I-G R (k)Hi(k) 


PrM) 
end loop 

= 

^(k)P R (k) 

25 
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7.2 Simplified Articulated Body Forward Dynamics Algo- 
rithm 

The complete recursive articulated body forward dynam- 
ics algorithm for a serial flexible multibody system follows 
directly from the recursive implementation of the expression 
in Eq. (57). The algorithm consists of the following steps as 
illustrated in FIG. 4: (a) a base-to-tip recursion as in Eq. (45) 
for computing the modal spatial velocities V m (k) and the 
Coriolis and gyroscopic terms a m (k) and b m (k) for all the 
bodies; (b) computation of the articulated body quantities 
using Eq. (78) and Eq. (63); and (c) a tip-to-base recursion 
followed by a base-to-tip recursion for the joint accelera- 
tions X as described below and illustrated in FIGS. 5a and 
5b: 




ZR + ( 0) = 0 

for k 

= 

1 ...N 

z(k) 

= 

\ 

\zM) ) 


= 

A {kMth k - 1 )zrM - 1) + b m (k) + K m (k)m 77 « 

<#) 


tm ) - zM) + in d m*zM) 

v#) 

= 

Uj l (k)^M) 

Z R (k) 

= 

z r (k) + S (k)eM) + PR^amRik) 

£R(k) 

= 

T T {k) - Ht{k)z R (k) e^*) 

v R (k) 

= 

rr R \k)e R (k) e^rik) 

ZR + (k ) 

- 

z R (k) + G R (k)e R (k) e^ 6 

end loop 




(64) 


a m (N + 1) = 0 
for k = N... 1 

a R +(k) = <t>*(f* + i, k) A *{k + \)a m (k + 1) 

m = v R (k)-G R *(k)a R +(k)e®Mk) 

«*(*) = a-R + (k) + Ht*(k)m + «■*») 

n(*) = Vj{k)-8\k)a R (k)e^m(h) 

a„(k) = ( n(k \. 


end loop 
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The recursion in Eq. (64) is obtained by simplifying the 
recursions in Eq. (58) in the same manner as described in the 
previous section for the articulated body quantities. The 
rigid Coriolis and centrifugal acceleration a mi? (k) is given in 
Appendix C below herein. 

In contrast with the composite body forward dynamics 
algorithm described in Section 5, the articulated body for- 
ward dynamics algorithm does not require the explicit 
computation of either M or C. The structure of this articu- 
lated body algorithm closely resembles the recursive articu- 
lated body forward dynamics algorithm for rigid multibody 
systems described in references ([15, 1]). 

The articulated body forward dynamics algorithm has 
been used to develop a dynamics simulation software pack- 
age (called DARTS) for the high-speed, real-time, hardware- 
in-the-loop simulation capability for planetary spacecraft. 
Validation of the DARTS software was carried out by 
comparing simulation results with those from a standard 
flexible multibody simulation package ([6]). The results 
from the two independent simulations have shown complete 
agreement. 

A System Embodying the Invention 

Referring to FIG. 6 , a robot manipulator 100 having 
flexible links (bodies), such as the manipulator illustrated in 
FIGS, la and lb, includes joint servos 110 controlling 
respective articulating hinges of the manipulator. A robot 
control computer 120 includes a processor 125 which com- 
putes the articulated body quantities of the manipulator 100 
from the current state of the manipulator 100 using the 
process of FIG. 2 . The current state of the manipulator 100 
is also used by a processor 130 to compute the Coriolis and 
centrifugal accelerations and gyroscopic forces of the 
manipulator links using the algorithm of Equation (44). A set 
of link (body) forces is specified to a processor 135 . The 
processor 135 uses the specified body forces, the articulated 
body quantities computed by the processor 125 and the 
gyroscopic and Coriolis terms computed by the processor 
130 to compute the deformation acceleration of the finite 
element nodes of each link (body) and the acceleration of 
each hinge by executing the algorithm of FIGS. 5a and 5b. 

In one embodiment of the invention described above with 
reference to FIG. 4 , the processor 135 repeats its operation 
over successive time steps, and the configuration vectors of 
the manipulator 100 required by the processors 125 and 130 
are computed by a processor 140 from the accelerations 
computed by the processor 135 for the previous time step. In 
an alternative embodiment of the invention, the hinge con- 
figuration vectors are derived by the processors 125 and 130 
directly from joint sensors 142 on the hinges of the manipu- 
lator 100. 

As one example of the application of the results computed 
by the processor 135 , a desired robot motion is defined by 
a set of user-specified node deformations and hinge accel- 
erations for a succession of time steps. The node deforma- 
tions and hinge accelerations computed during each time 
step by the processor 135 are compared by a processor 144 
with a desired user-specified node deformations and hinge 
accelerations for the corresponding time step to determine 
an error and to correct the specified body forces to reduce the 
error using well-known feedback control techniques. Such 
feedback control techniques are well-understood in the art 
and need not be described here. The corrected body forces 
are then stored for later (or immediate) conversion by a 
processor 146 to joint servo commands for transmittal to the 
joint servos 110. 

8. Computational Cost 

This section discusses the computational cost of the 
composite body and the articulated body forward dynamics 
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algorithms. For low-spin multibody systems, it has been 
suggested in [16] that using ruthlessly linearized models for 
each flexible body can lead to significant computational 
reduction without sacrificing fidelity. These linearized mod- 
5 els are considerably less complex and do not require much 
of the modal integral data for the individual flexible bodies. 
All computational costs given below are based on the use of 
ruthlessly linearized models and the computationally sim- 
plified steps described in Appendix B. 

10 Flexible multibody systems typically involve both rigid 
and flexible bodies and, in addition, different sets of modes 
are used to model the flexibility of each body. As a conse- 
quence, where possible, we described the contribution of a 
typical (non-extremal) flexible body, denoted the k th body, to 
15 the overall computational cost. Note that the computational 
cost for extremal bodies as well as for rigid bodies is lower 
than that for a non-extremal flexible body. Summing up this 
cost for all the bodies in the system gives a figure close to 
the true computational cost for the algorithm. Without any 
20 loss in generality, we have assumed here that all the hinges 
are single degree of freedom rotary joints and that free-free 
assumed modes are being used. The computational costs are 
given in the form of polynomial expressions for the number 
of floating point operations with the symbol M denoting 
2g multiplications and A denoting additions. 

8.1 Computational Cost of the Composite Body Forward 
Dynamics Algorithm 

The composite body forward dynamics algorithm 
described in Section 5 is based on solving the linear matrix 
30 equation. 

M X-T- C 

The computational cost of this forward dynamics algorithm 
is given below: 

35 1. Cost of computing R(k) for the k th body using the 

algorithm in Eq. (50) is 

[48n m (*) + 90]M + [n m 2 (k) + Winjk) + 116JA. 

2. Contribution of the k th body to the cost of computing 
40 M (excluding cost of R(k)’s) using the algorithm in 

Eq. (50) is {k[12n m 2 (k)+34n m (k)+13]}M+{k 
[lln m 2 (k)+24n m (k)+13]} A. 

3. Setting the generalized accelerations X=0, the vector 
45 C can be obtained by using the inverse dynamics 

algorithm described in Eq. (45) for computing the 
generalized forces T. The contribution of the k //2 body 
to the computational cost for c(k) is {2n m 2 (k)+ 
54n m (k)+206} M+{2n m 2 (k)+50n m (k)+143} A. 

50 4. The cost of computing T-c is { M} A. 

5. The cost of solving the linear equation in Eq. (46) for 
the accelerations X is 

{1/6W 3 + %W 2 +{i/fiW 3 + W 2 - 7 /e N}A. 

The overall complexity of the composite body forward 
dynamics algorithm is 0( jV 3 ). 

8.2 Computational Cost of the Articulated Body Forward 
Dynamics Algorithm 

60 The articulated body forward dynamics algorithm is based 
on the recursions described in Eq. (78), Eq. (63) and Eq. 
(64). Since the computations in Eq. (78) can be carried out 
prior to the dynamics simulation, the cost of this recursion 
is not included in the cost of the overall forward dynamics 
65 algorithm described below: 

1. The algorithm for the computation of the articulated 
body quantities is given in Eq. (63). The step involving 
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the computation of D~*(k) can be carried out either by 
an explicit inversion D(k) with 0(n m 3 (k)) cost, or by 
the indirect procedure described in Eq. (63) with 
0(n m 2 (k)) cost. The first method is more efficient than 
the second one for n m (k)^7. 

Cost of Eq. (63) for the k //z body based on the explicit 
inversion of D(k) (used when n m (k)^7) is 

{ 5 /6n m \k) + is/mjik) + + 180}M + 

{Ven m 3 {k) + 2 Vzn m 2 (k) + 5A Vm m (k) + 164} 

Cost of Eq. (63) for the k th body based on the indirect 
computation of D _1 (k) (used when n m (k)^8) is 
{ 1 2n m z (k)+255n m (k)+572} M+{ 1 3n m 2 (k)+l 82n m (k) 

2. The cost for the tip-to-base recursion sweep in Eq. (64) 
for the k"‘ body is {n„ z (k)+25n m (k)+49} M+{n m 2 (k)+ 
24n m (k)+50} A. 

3. The cost for the base- to -tip recursion sweep in Eq. (64) 
for the k th body is {18n m (k)+52{19n m (k)+42{ A. 

The overall complexity of this algorithm is 0(Nn m 2 ), where 
n m is an upper bound on the number of modes per body in 
the system. 

From a comparison of the computational costs, it is clear 
that the articulated body algorithm is more efficient than the 
composite body algorithm as the number modes and bodies 
in the multibody system increases. The articulated body 
algorithm is faster by over a factor of 3 for 5 modes per body, 
and by over a factor of 7 for the case of 10 modes per body. 
The divergence between the costs for the two algorithms 
becomes even more rapid as the number of bodies is 
increased. 

9. Extensions to General Topology Flexible Multibody Sys- 
tems 

For rigid multibody systems, [11] describes the extensions 
to the dynamics formulation and algorithms that are required 
as the topology of the system goes from a serial chain 
topology, to a tree topology and finally to a closed-chain 
topology system. The key to this progression is the invari- 
ance of the operator description of the system dynamics to 
increases in the topological complexity of the system. 
Indeed, as seen here, the operator description of the dynam- 
ics remains the same even when the multibody system 
contains flexible rather than rigid component bodies. Thus, 
using the approach in [11] for rigid multibody systems, the 
dynamics formulation and algorithms for flexible multibody 
systems with serial topology can be extended in a straight- 
forward manner to systems with tree or closed-chain topol- 
ogy. Based on these observations, extending the serial chain 
dynamics algorithms described in this specification to tree 
topology flexible multibody systems requires the follow 
steps; 

1. For each outward sweep involving a base to tip(s) 
recursion, at each body, the outward recursion must be 
continued along each outgoing branch emanating from 
the current body. 

2. For each inward sweep involving a tip(s) to base 
recursion, at each body, the recursion must be contin- 
ued inwards only after summing up contributions from 
each of the other incoming branches for the body. 

A closed-chain topology flexible multibody system can be 
regarded as a tree topology system with additional closure 
constraints. As described in [11], the dynamics algorithm for 
closed-chain systems consists of recursions involving the 
dynamics of the tree topology system, and in addition the 
computation of the closure constraint forces. The computa- 
tion of the constraint forces requires the effective inertia of 
the tree topology system reflected to the points of closure. 
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The algorithm for closed-chain flexible multibody systems 
for computing these inertias is identical in form to the 
recursive algorithm described in [11]. 

10. Conclusions 

This invention uses spatial operator methods to develop a 
new dynamics formulation for flexible multibody systems. A 
key feature of the formulation is that the operator description 
of the flexible system dynamics is identical in form to the 
10 corresponding operator description of the dynamics of rigid 
multibody systems. A significant advantage of this unifying 
approach is that it allows ideas and techniques for rigid 
multibody systems to be easily applied to flexible multibody 
15 systems. The Newton-Euler Operator Factorization of the 
mass matrix forms the basis for recursive algorithms such as 
those for the inverse dynamics, the computation of the mass 
matrix, and the composite body forward dynamics algorithm 
for the flexible multibody system. Subsequently, we develop 

20 

the articulated body forward dynamics algorithm, which, in 
contrast to the composite body forward dynamics algorithm, 
does not require the explicit computations of the mass 
matrix. While the computational cost of the algorithms 
25 depends on factors such as the topology and the amount of 
flexibility in the multibody system, in general, the articu- 
lated body forward dynamics algorithm is by far the more 
efficient algorithm for flexible multibody systems containing 
even a small number of flexible bodies. All of the algorithms 
30 are closely related to those encountered in the domain of 
Kalman filtering and smoothing. While the major focus in 
this specification is on flexible multibody systems with serial 
chain topology, the extensions to tree and closed chain 
35 topologies are straightforward and are described as well. 

While the invention has been described in detail by 
specific reference to preferred embodiments thereof, it is 
understood that variations and modifications may be made 
without departure from the true spirit of the invention. 

40 Appendix A: Proofs of the Lemmas 

At the operator level, the proofs of the lemmas in this 
publication are completely analogous to those for rigid 
multibody systems ([1, 2]). 

45 Proof of Lemma 5.1: Using operators, we can rewrite Eq. 
(47) in the form 

M m =R~e^R€% (65) 

50 From Eq. (19) it follows that = £o <I>=®-I=tI>. Multi- 
plying Eq. (65) from the left and right by and <$* 
respectively leads to 

Proof of Lemma 6.1: It is easy to verify that TPt^^P. As a 
consequence, the recursion for P(.) in Eq. (51) can be 
rewritten in the form 

60 

M m =P- PC V p " e P€*o +KDK* (66) 

Pre- and post-multiplying the above by 3> and 9>* re spec - 
65 tively then leads to 

®M m ®*=P+®P+P® *+®KDK*<& * 
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Hence, 

=* M = n * = ^[P + 0P + Pi* + O/sTjDAT^O*] n * I 

= D+ n ®KD + DK*&* H * + ' H ®KDK*Q>* H * = 

[/+ H <WqD[/ + w <wq* 5 

Proof of Lemma 6.2: Using a standard matrix identify we 
have that 
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model fidelity when the deformation and deformation rate 
dependent terms are dropped altogether from the dynamical 
equations of motion ([16]). Such models have been dubbed 
the ruthlessly linearized models. These linearized models 
are considerably less complex, and do not require most of 
the modal integrals data for each individual flexible body. In 
this model, the approximations to M m (k), a,„(k), and b 7 „(k) 
are as follows: 


[/+ wojq-w- hoii+k n or 1 k 


Note that 


¥ -W-e^(/- €&)+ £&G H^ l +K H 


from which it follows that 


(67) 


10 


( 68 ) 

15 





, and b m (k ) * b m °(k) 


(75) 


With this approximation, M w (k) is constant in the body 
frame, while a w (k) and b m (k) are independent of rj(k) and 
T|(k). With this being the case, the formation of D" 1 in Eq. 
(51) can be simplified. Using the matrix identity. 


[A+BCB*r l =A~ 1 -A- 1 B[Cr 1 +B*A~ 1 BT 1 B*A (76) 


yf^I+KHO 

Using this with Eq. (67) it follows that 


which holds for general matrices. A, B and C, it is easy to 
verify that 

(77) 


{/+ H<&K]- l =l- HyK 

Proof of Lemma 7.1: From Eq. (42) and Eq. (43), the 
expression for the generalized accelerations X is given by 25 

X=M~\T-C)-[I~ HyK}*D- l {I- H\yKJ[T~ H0[M m ®*a m +b m + 


K m v\] (69) 

From Eq. (68) we have that 30 

[/- HyK\ ttylxr 1 -# n ]<&= Ky (70) 

Thus, Eq. (69) can be written as 35 

Ml- HyK}*D- l [T- Hy{FCr+M m 0*a m +b m +K m v]] (71) 

From Eq. (66) it follows that 

40 

M m =P- € W P e *4>->yM m 0*^P+Pi* (72) 

and so Eq. (71) simplifies to 


*=[/- HyK] *D~'[T- ny{KT+Pa m +b m +K m v}- HP$*a m \ (73) 45 

From Eq. (68) we have that 

[/- HyK]*D~ l HP0*=[I- HyK]*K*0*=K*y*[y~*-K H )*<&*= 5 ° 

K*y* (74) 


where the matrices A(k), Q(k), and Y(k) are precomputed 
just once prior to the dynamical simulation as follows: 


for A: 

= I...N 

Aflfc) 


m 

- ^j(k) (k) A"x6 

Y (*) 

= A 

W) 

= ^*(k)Y(k) e^ 6x6 

end loop 



(78) 


Using Eq. (77) reduces the computational cost for comput- 
ing the articulated body inertias to a quadratic rather than a 
cubic function of the number of modes. 

Appendix C: Expressions for M m (k), a m (k) and b m (k) 

The modal spatial displacement influence vector IF(k) for 
node j*has the structure: 


r m 


( m) \ 

) 


9l6xn m m 


(79) 


The components of the vectors V(k) e ^ 3xn “<*> and Y(k) e 
^3xn m (k) ^ t ^ e m0( j a i s } 0 p e displacement influence vector 
and the modal translational displacement influence vector 
respectively for node j*. They define the contribution of the 
various modes to the slope (or differential change in orien- 
tation) and translational deformation for the j k th node on the 
k' A body. Define 


8 ra (j*)AX;(k)ii(k) e & 3 , 8 v (j*)A Y(k)ri(k) £ and 

6/(j.)^y(k)n(k)€ St 3 


(80) 


Using this in Eq. (73) leads to the result. 

Appendix B: Ruthless Linearization of Flexible Body 55 
Dynamics 

It has been pointed out in recent literature ([17, 16]) that 
the use of modes for modeling body flexibility leads to 
“premature linearization” of the dynamics, in the sense that 
while the dynamics model contains deformation dependent 60 
terms, the geometric stiffening terms are missing. These 
missing geometric stiffening terms are the dominant terms 
among the first-order (deformation) dependent terms. In 
general, it is necessary to take additional steps to recover the 
missing geometric stiffness terms to obtain a “consistently” 65 
linearized model with the proper degree of fidelity. However 
for systems with low spin rate, there is typically little loss is 


Note that 


Kk,h)=l ofoWifo) 

where \ 0 Q k ) denotes the undeformed vector from frame 
T k to node j*. Note that M/j*) denotes the spatial inertia of 
the ) lh node on the k ?/ * body and is given by 


M s (jk) = 


( 


T o*> 

- m(jk)p(jk ) 


m(Jk)p(jk) 



(81) 


C.l Modal Integrals for the Individual Bodies 
Defined below are a set of modal integrals for the k lh body 
which simplify the computation of the modal mass matrix 
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M m (k) and the bias vector b m (k). These modal integrals can 
be computed as a part of the finite-element structural analy- 
sis of the individual bodies. 


«,*(*) 

m{k) A £ m(j k ) 
j=h 

n s (k) 

Po k A tl/m(*)l . £ m(j k )[p(j k ) + UkJ k )) e 513 
7= U 

",(*) 

Mr) A [!/«(*)] I rntfOy^e * 3 
7=1* 

n/ 1 ) 

A £ e 513 

7=1* 

t _ _ 
foV) — . 2 T (/*)V(1) + m{jk)Uo(k, j k ) + p(jkW{k) ~ m(j k )k(k, jk)p(jkW(k ) e 
7=1* 

n s (k ) 

Ti*(r, s) A . £ m(/*)y/(l)[Y7(l) -/>(/*) V(*)l e 513 
7=U 

n s(k) 

C^ir.s) A £ IV( 1 )]* ‘(/*)V(&) + «(/*)[ VWl^pO’OyA*) + m(/*)[Xy(*)]*^(/ifc)Yr , '(*) w(/Jt)[Y^(*)]*YAfc) e 511 
7=U 

T rtj ^T - - _ 

0 * — - £ (jk) - m(jk)lk(k, jk)k(k, jk) +p(jk)lo(k, jk) + k(k j k )p(j k )] e ^ 3x 
j=l k 

_ n s (k) 

T i Hr) - . £ m(jkyymWk,jk)+p<j k ) 1 e 513x3 

7=1* 


V(r, 5) = - £ m(ji)yA%/We^ x3 

y=i* 

Mr) A 4 £ [m(j k )p(j k )XAk)] x k(k,jk)- T (/*)V(Jfc) £ 513x3 

7=U 

«*(&) 

Mr. j) A £ [m 0 ^ 0 fc)W)JV(it) 6^ 3x3 
7=1* 

n s (k) _ . 

*V(r) = £ 2/ 0 (^ 7*) [ m(/*)/> (/*) V(*)F - ' (/*)V(&) + V(*) T (/*) e 513x3 

7=1* 

= 2[5,*(r)]* - . £ Mk) T (jk) + T (/*) V(*)] 

7=1* 

n s (k) 

Mr. 5 ) A £ 2y^)[w(/*)pO'dV(fc)l x = 2[Mr.j)] # e 5l3x3 
7 =1 * 

«*(*) _ 

/fl‘(r) = .Z T (i»)£* ! 

7=U 

«.s(*) - _ 

Mn 5) A £ I V'(*) ' (/*) - «(/*)*,(*; jM(k)p(j k )]XJ(k) e^ 3 
7=1* 

n 5 (*) 

M<?, r,j) A 2 -m(/ fc )Y^)V(%(/*)V(ik) e 513 
7=1* 

«j(*)- 

Mr. i)A I V(*)mO*)p(/*)Y^) e 513 

7—1* 

*#)- _ 

Mr. s) A £ MMoWWe 513 
7=1* 

n s (k) . 

LHr.s) A-[i/ m( jfe)] £ m(j k )XAk)~p(j k )Xf(k) 

7=1* 

« j (*) _ _ . 

T\ k (r t s) A £ [mO*)yA%(/*) + ^WWM^e 313 
7=U 
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n s (k) 


Ti k (r, s) — 2 f 
J=h 

«#) _ _ ~ 

W* r. j) ± 2 rV(fc)]*[m(/ A )y;(%(/-t) + T W(*)M*) e 511 


Note that 

G*(r,s)=G*(s,r) and *^ 2 A: (r,s)=^ r 2 fc (s,r) 

Also define, 

n w (&) 

/>(D -p0* + 2: p](s)T\(s) £^ 3 

5=1 

«m(D 

F*(r)=F 0 fc (r)+ 2 F^(r, 5)ri(5) e^ 3 

5=1 


10 


Hence, in block partitioned form 


(83) 


A Hr) = 


T *«(*) — 

V(ri + 2 Vfo *s)nCs) 

.5=1 


15 


20 


* p $3x3 


T (i) = T o* + £ [ T i*M + {V(r)}*]il(r) + 

r= 1 


25 


«m(D n m (k) 

2 2 T 2^5)Tl(r)TKj)6^ 3x3 

r=l 5=1 


n m (k) 

S k (r) = S\ k (r) + 2 S 2 k (r, 5 ) 11 ( 5 ) e 913x3 
5=1 

n m (k) 

K k {r) = Kfyr) + 2 K 2 *(r, 5)n(D e 913x3 

5=1 

n m (^) 

F*(r, 5 ) — ^(r, 5 ) + 2 Ri k (q, r, s)r\(q) e 9 * 3 

5=1 



^ Gk 

i^]* 

[£*]* 

M m (k ) = 

F k 

J (k) 

m(*)p(A:) 


U 

-m(k)p(k) 

m(k)I 


1 G k 

IF. 6*1* 

[E k ]* 

= 

Fo k 

T o‘ 

m(k)p 0 k 


, E k 

-m(k)p o* 

m(Jk)I 



Mm(k) 


/ 0 


[Fi*nl* 



30 


Fi^n 


35 \ 


n m (k) 

2 [ T i*(r) + l ^V(r)]*]T|(r) 
r=l 


MmKk) 


( 86 ) 


C.2 Modal Mass Matrix 

We have from Eq. (23) that the modal mass matrix of the 
k ih body is given by 


mmpMk)) x 


M m (k) 


{ U m ) w.b*(*)i 

/ Mgfjk) Mgffi 


U*(k)M s (k) (k) n*(k)M s (k)B*(k) 

B{k)M s (k) (k) B(k)M s (k)B*(k) 


(84) 


(85) 


I £ $ 33 (k)X 33 (k) 

\ M&> K(k) J 

Define the matrices: 

pf £ Ip AD. • • ■ pAn«(*))l e9t 3xnm(k) 

F 0 k A [/?AU, . . . F 0 \njm e$ 3xnm(k) 

F* a [p*(i), . . . £*(«„(*))] 6 sjU xnm(k) 

E k A [£*(1), . . . £*(«„(*))] e^ 3xrim(k) 

Also define the matrix G* e so that its fas)'* 

element is given by the modal integral G*fas). 

Using these matrices, and Eq. (84), it is easy to establish 
that 


50 


55 


A #(*) = <?*, 




and 


M r '(k) = 


T W m(k)p(k) 

-m(k)p(k) m(k)I 


60 


65 


-continued • 


/ o 


n m (k) n m {k) 

0 2 2 T 2 fc (r,j)n(r)Tl(5) 

r= 1 5=1 


0 

Mm\k) 


o\ 


The superscript i=0,l,2 in M w *(k) denotes the order of 
dependency of the terms on the deformation variables. 
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C.3 Expression for a m (k) to the angular accelerations and the term at the bottom to the 

In this section we derive explicit expressions for the j inear acceleration of the body. Also 
Coriolis and centrifugal acceleration term a m (k). Since 

, , , /° kx,y) \ 5 

* fcy)= lo 0 j 

it follows from Eq. (12) and Eq. (79) that 


a>(fc+U) 


( o [n»(Jk + 
0 <j>(Jfc+l ; 


+ i)]*<|>(/* + i,fc) + imk + \)]mt M ,k) 

k) 


0 [K(k + 1)]* [V(k + l)m k+ uk) + [Y(fc + 1)]* + [V(k + l)]*Ktk+i,k 
0 0 l(k+ l,k) 

i 0 0 0 


Recalling that the spatial velocity of frame T k is 


20 


V(k) = 


\v(*) 


where to(k) and v(k) denote the angular and linear velocity 25 
respectively of ? k we have that 


n <(*) = 


m) 


a>(k) }J(k) 
kkW) 


And thus 


®*(k+l,k)V m (k+l) = 


w(k + l)5 m te+i) 

-l(t k+i ,k)(kk + l)5cj(f*+i) + a(k + l)5v(/it+i) 
+5 <a (fj t+ i)i(r fc+ i,fc) + co(* + 1 )l{k + l,k) 


40 


The vector above has bee partitioned so that the term on the 
top corresponds to modal accelerations, the term in middle 


Kk + i j k+J ) = u{k + i)/(* + l J k+J ) + 5 V {j k+J ) 
and 

kt m Jc) = rnUhim.uk) + A„(t) - 6v(4) + K,{k)l( \k) 

= [ceK* + 1) + + A v (i) - 8v(*) + A „,(*)« 0 t,k) 

where 


A v (*) 

Thus 


/ A u (&) \ 

1 \ A v (k) J = 


( 87 ) 




l(k + l,k) = Kk + lJ M ) + KlM,k) 

= m(k + l)l(k + U) + 8v((*ti) + hj.iM)l(tM,k) + A v (i) - 8v(4) + A dk)l( &t,k) 

Also 


H* {k) ±(° . ° \ 

I - n d (k) Ht*(k) J 


and 
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-continued 

/ k°k) 0 \ 

"* w = (o 

l(O a k) = aXOMO^k) - 
Thus we have that 


H *(k)x(k) = ( 


0 

a>(0 k )AM-a>(Q5M) 


\ 5( ^)Av(i) - 5(4)8^4) + A*,(4)i( #*,4) - 7( ® k Jc)k °k)^(k) 


! 


From Eq. (25) and the above expressions it follows that 


mb- MHk i l ’ k) vuk + i )+ -i^a_ *». 


0 


where 




m(* + l) + M(*)A ffl (*) - G)( 

5(4 + 1)[««(* + 1 m + U) + 2Sv(fj:+i)] + (wdtri) + k ®k)\ [Vft) - v( <V) + 
(5(4 + 1) + 8„(<t + i)]W«t + i)/(/w^) - [2A M ~ 8o(4)]5 v(</t) 


/ 5(4)A ra (4) 

I 5(4 + + l)/ 0 (4 + 1,4) + [5(4 + 1) + co(4)]|v(4) - v( @ t *)] 

iW 

+ 5(4 + 1)(5(4 + l)(S,fe +i ) - 6,(<4)1 + 28 v (<* + ,)] 

+[S m (W) - 5 01 (4)](v(4) - v( <V)1 
+ 5(4 + l)SJ.t t+ Mt k+ ,Jc) - 2A U (4)8 V W*) a U(.k) 


+ 


- - ° ) 
(a(k + l)5 M (/jt+i)5/(^ + i + &a>(tk+l)$a>(.tk+\)k{tk,k - 1) + SuWSuWSvW J 


( 88 ) 


( 89 ) 

( 90 ) 


0 £*(*) 


f- - ° 

\ 5 (0 (^ + i)8 t fl(//fc + i)8/(/jfc+i,/:) 


<£*(*) 


(91) 


In the above a m/? °(k) denotes the deformation independent 
part of the Coriolis acceleration, while a^Ck), a mff 2 (k) and 
a mJ? 3 (k) denote the parts whose dependency on the defor- 
mation is up to first, second and third order respectively. 55 
C.4 Expression for b m (k) 

We have found Eq. (28) that 


<*(/*) = 


d[nKkW&jk)) 

dt 


v m (k ) = mMk) + rfrhmk) 


60 


Since, 

kkjk)=<o(k)l(k,j k )+S v (j k ) 

it follows that 

( <&(£) So)(/d 
- - 


65 
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Also from Eq. (31) we have that 
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b(jk) 


/ (0(/a) T (jk)®(jk) 

\ m(jk)vQk)®(jk)p(jk) 


Thus, 

bijk) + Afj (/*)«*(/*) = 


I c o(/t) ^(/*)w(/*) + T Wa) + 25v(/*)] 
'"(/it) {-p(/jt) w W8a)(/jt) + &jjk)§<a(jk)p(jk) + 8 Q (/'0w(^)p(/it) + 

mMk){ l(k,j k ) + P m + SMpdk) + 25v (/*)]} 


(92) 


From Eq. (37) we write 
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- »*(*) lmQ t )p(j i )KKk)r~l(k,j k Mk) 

(95) 


I N*(D \ 



+ <a*(k)m(jJy r Kk){l(k.jJ + />(/*) }<oW 

(96) 


; 


20 

+ 2m*{k) [«(/>(/'*) V(«f5v0.) 

(97) 

brnVc) = ( n *® ) m) + MAM®) = 

; b n k (n m (k)) 



- 

(98) 


| bj< 




(99) 


\ » / 
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(100) 

We develop expressions for b^r), b^ 

k and b v * in Eq. (93) 



(101) 

below. From Eq. (92) and Eq. (93) we have that 









(102) 

n s (k) . 


(94) 
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bjf(f)= ? -co*(WW T (/'«)- 





(103) 

i=ljt 




+ n ‘(jk>b a ,(j k )*y t Kk)p(j t )b a (j k ) 

(o*(k)%,Hk) 3 <j k )&Jj k ) - S M*%Xk) 3 (/*)«>(*) - 



+ 5 a (j t )*3u t )Vmjj t ) 

(104) 

SM*V(Q 3 (j.l&M - lK‘m* 3 (jtfjjMk) + 
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+ m(; t )iy r j W]*8 m (/t)S(*:)P(/ i ) 

(105) 


®*(*) N(/>(/*)V(*)l x [-7 (*,A)u>(*) + 25 v (/jt)] + 

mO*)(o8(it)*Y r j(*)H7a;*) + p(/*)}a>(fc) + 28 v (/*)l + 
m(/*)[YA*)] # 15 w (/.)5co(/X/7) + 

5<a{jk)V>(k)p(j k ) + ^(k)ljj k )p(j k )} = 


+ m(A)lT, J WI*(O«:)8o,0/ i )p(/ t ) (106) 

Using the modal integrals defined in Section C.l, the 
40 above terms can be expressed in the following manner: 
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n s (k) m 

X -0J*(i)V(i) T (/*)t0(i) 

y=u 


i «/»(*) 

4- [97] + 102 = -co*(fc) £ ri*(j,r>ii(s) 


«mGt) 

2[97] + 99 + 101 = -©*(*) I [72 ft (j,r) + TVi*(r,s) + W^Cs.r)]^) 

5=1 

94 + 95 = -tD*(Jfe)S fc (r)(D(Jfc) 

96 = -to*(k)N\rMk) (107) 

«m(^) n m (k) 

103 + 104 = z s 

9=1 5=1 

100 + 105 = 106 


n m(k) 

98 + 100 + 105 + 106 = -2o)*(fc) £ Fi i ( 5 ,r)n( 5 )Using these, it follows that 
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n m (k)n m (k ) . . (108) 

V = -C 0 (fc)[S*(r) + N k (r)]<a{j k ) + Z Z T 3 *(^5)n(tf)Ti(5) - 

<7=1 5=1 

n m (k) 

(&*(k) Z + T 2 k (s,r) + Wfyr,s) + W 2 \s,r ) + 2Fi*(j,r)]Ti(s) 

n m (k) n m {k) n m (k ) 

= -m*(/:)[S*(r) + M(r)]co(/jt) - ©*(*) Z Q k (r,sMs)+ Z Z 7 3 *(*r.ff)n C?)nC®) 

y=5 <p 1 5=1 

where 

r/(j,r) + T 2 k (s,r) + W/to) + W/(j,r) + 2F/(j,r) (109) 


Once again from Eq. (92) and Eq. (93) we have that 


«*(*)_ T T _ ... 
ba k = ? toO'i) T fo)*Dfo) + T ijicMWA) + m(/jO!P(/*)co(*)(to(A:)/(ftJjt) + 25v(/jt)] + 

j=h 

m(jk)l(jk ) {-/?(/dw(/:)5 M (/ t ) + w(*)lco(fc){ /(*,;*) +/?(/*)} + 25v(fe)] + 

S«>(jk)5m{jk)p(jk) + §<s>(jkMk)p(jk) + W(k)5®{jk)p{ik)} 

n s (k) _ . _ _ _ 

= Z oX*)[ ' (jk) - m(jk)(p(jk)l(k,jk) + l(jk)p(kjk) + l(jk)Kk,jk)Wk)) 

7=1* 

- 2 mtf*)[T(/*) + ?(/*)] 5 v (/ ft )co(*) 

”^)5co(/ A )0)W 
+ ScoC/fc) 3 (it)co(^) 

+ i0*)m(/4)F(/*)8»0'*)“(*) 

+ B(fc)J(/*)5 ra (/*) 

+ ^ (/*) w(/*)5 m (/*)5 m 0‘*>P (/*) 

+ m(j k jl(j k )<kk)djjdp(j k ) 


( 110 ) 


( 111 ) 

( 112 ) 

(113) 

(114) 

(115) 

(116) 

(117) 

(118) 
(119) 


Once again, using modal integrals, the above terms can be 35 
reexpressed in the following manner: 


110 = <»*(*) ^ (*)©(*) 


( 120 ) 


111 = 


n m (k) 

2 Z JV*(r)T|(r) 
r=l 


«(*) 


n m (/:) 


112+113 + 114 + 118+119= Z ^(r)Ti(r)co(jt) 
r=l 

„ n m (k) 

115 = co(A:) Z R\ k (r)r\(r) 
r=l 
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ti m (k) n m (k) 

116 + 117= Z Z R k (r,sMr)T\(s) 
r= 1 j=1 


This results in the following expression 


_ n m (k) (121) 

b m k = ©(*) T (jfe)(o(Jfc) + Z [21V*(r) + ^(r)]Tj(r)co(fc) + 
r=l 

n m (&) n m (k) n m (k ) 

m(Jfe) Z /?i*(r)Ti(r)+ Z Z F*(r,j)n(r)ri(j) 
7=1 7=1 5=1 


Using Eq. (92) and Eq. (93) it also follows that 


n s (k) _ _ 

b v k = Z — m(/*)p(/*)o>(*)5to</jfc) + m(/*)<o(£)[G)(fc){ l(k,j k ) + /?(/'*)} + 25y (/*)] + 
7=1* 

m(jk)§u>(jk)&(a<jk)p<jk) + m(jk)$Jjk)®(k)p(jk) + m(jk)®(k)Su(jk)p(jk) 

n s {k) _ _ 

= 2 - m(jk)p{jkMk)$u(jk) 

y-U 


+ «(/jftjk)5tt){W*) + p(/*)} 

+ 2m(/*)coCA:)6 v (/j t ) 

+ wO'^ScoO^^fC/a) 

+ m(/ fe )co(^)5 M (/ fc )/70V) 


( 122 ) 


(123) 

(124) 

(125) 

(126) 
(127) 
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Using the model integrals we have that 

123 = m(kMkMk)p(k) 

Wm(fc) ^m(^) 

124 = m(k) Z Z' L(r,s)T[(r)r\{s) 

r= 1 5=1 

n m (k) 

122 + 125 + 126 + 127 = 2co(fc) I £*(r)r|(r) 
r=l 

10 

and thus 

n m (k) (129) 

b v k = m(kMkMk)p(k) + 2co(fc) I £*(r)T|(r) + 
r=l 

15 
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(128) 


-continued 

nm(^) n m (k) 

m(fc) Z Z L(r,5)T)(r)n(5) 

r=i 5=1 


Putting together Eq. (108), Eq. (121) and Eq. (129) we have 
that 


- co*(i)(Si*(l) + T i*(l))o>00 


b m (k) 


- <a*(k)lS\ k (n m (k)) + T ,t( n „(i))](D(i) 
oi(k) T 0 t(o(A:) 
m(k)co(k)G)(k)po(k) 
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! 


b m \k) 


n m (k) . _ 

-CO *(*) Z [£*(1,^) + {^(1,5) + V(l,5)}TlCs)] 
j=s 


mih ) . _ 

-«>*(*) z [^(nM^Ms) + {S 2 k (n m {k),s) + 1 2 k (n m (kls)Ms)] 

j=s 


n m (k) 
Z 
r= 1 


\ 


[£>(*){ T i‘(r) + [ T i*(r)]*}Tl(r)o>(fc) + {2[ T ,‘(r)]» + AT*(r) }ii(r)<o(fc) + tfi)RAr)n(r)] 
co(fc)[m(fc)co(fc) p\ k r\ + 2E k i\] 


b m l (k) 

/ n m (k ) n m {k) 

z z r 3 *(?.u)n(?)n(j) 

q=\ 5=1 


n m (k) n m {k) 

Z Z T 3 k (q,n m (k),sMqMs) 
q=\ 5=1 


n m (k)n m (k) T . . . 

Z Z [co (k) ' 2 k (r,s)(a(k)T\(r)r\(s) + 1 2 k {r,sMk)T[{r)j\(s) + R 2 k (r,s)r\(r)Tf\(s)] 
r= 1 5=1 

%(t) w m (/:) 
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What is claimed is: 

1. A method for controlling a manipulator relative to a 
desired manipulator motion, said manipulator comprising 
plural bodies including an outermost body, and a relatively 
stationary innermost body, said plural bodies being sequen- 
tially connected together by movable hinges disposed 20 
between each plural body so connected and servos control- 
ling said movable hinges in accordance with servo command 
signals corresponding to specified body forces of respective 
ones of said plural bodies, at least some of said plural bodies 
being flexible in plural deformation modes corresponding to 25 
respective modal spatial influence vectors relating deforma- 
tions of plural spaced nodes of respective plural bodies to 
said plural deformation modes, said method comprising the 
steps of: 

30 

computing articulated body quantities for each of said 
plural bodies from respective modal spatial influence 
vectors; 

computing modal deformation accelerations of said plural 
spaced nodes of respective plural bodies and hinge 35 
accelerations of said movable hinges from said speci- 
fied body forces, from said articulated body quantities 
and from said modal spatial influence vectors; 

comprising said modal deformation and hinge accelera- 
tions with said desired manipulator motion to deter- 40 
mine an error, and correcting said specified body forces 
so as to reduce said error thereby producing corrected 
specified body forces; 

generating said servo command signals by converting in 
a processor means said corrected specified body forces 45 
to servo commands to correct manipulator motion to 
said desired manipulator motion, and transmitting said 
servo command signals to said servos. 

2. The method of claim 1 wherein said step of computing 
articulated body quantities comprises, for each body begin- 50 
ning at said outermost body: 

computing a modal mass matrix; 

computing an articulated body inertia from the articulated 
body inertia of a previous body and from said modal 
mass matrix; 55 

computing an articulated hinge inertia from said articu- 
lated body inertia; 

computing an articulated body to hinge force operator 
from said articulated hinge inertia; 60 

computing a null force operator from said articulated 
body to hinge force operator. 

3. The method of claim 2 wherein said step of computing 
a null force operator is followed by revising said articulated 
body inertia by transforming said articulated body inertia by 65 
said null force operator to produce a revised articulated body 
inertia. 


4. The method of claim 2 wherein said plural bodies and 
movable hinges are characterized by respective vectors of 
deformation and hinge configuration variables, and wherein 
said computing modal deformation accelerations and hinge 
accelerations comprises: 

for each one of said plural bodies beginning with said 
outermost body: 

computing a residual body force from a residual body 
force of a previous body and from said vector of 
deformation and hinge configuration variables, 
computing a resultant hinge force from said specified 
body force and said residual body force, 
computing a resultant hinge acceleration from said 
resultant hinge force transformed by said articulated 
hinge inertia; 

and, for each one of said plural bodies beginning with said 
innermost body: 

computing a current modal body acceleration of a 
current body from a modal body acceleration of a 
previous body, 

computing a modal deformation acceleration and hinge 
acceleration from said resultant hinge acceleration 
and from said current modal body acceleration trans- 
formed by said articulated body to hinge force opera- 
tor. 

5. The method of claim 4 wherein: 

said step of computing a resultant hinge acceleration is 
followed by the step of revising said residual body 
force by said resultant hinge force transformed by said 
body to hinge force operator to produce a revised 
residual body force for use in said correcting of said 
specified body forces; and 

said step of computing a modal deformation accelera- 
tion and hinge acceleration is followed by the step of 
revising said current modal body acceleration based 
upon said modal deformation and hinge acceleration 
to produce a revised current modal body acceleration 
for use in said correcting of said specified body 
forces. 

6. The method of claim 5 wherein all said computing 
comprises a single cycle corresponding to one of a succes- 
sion of time steps, all said computing being repeated for 
subsequent time steps, wherein said vector of deformation 
and hinge configuration variables are computed from the 
modal deformations and hinge accelerations of a previous 
time step and wherein the revised articulated body inertia, 
revised residual body force and revised current modal body 
acceleration from the previous time step are used for com- 
puting in a current time step. 

7. The method of claim 4 wherein said manipulator 
comprises joint sensors at each of said movable hinges, and 
wherein a hinge configuration portion of said vector of 
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deformation and hinge configuration variables is derived 
from reading said joint sensors. 

8 . The method of claim 4 wherein said articulated body 
inertia, said articulated hinge inertia, said body to hinge 
force operator, said null force operator, said specified body 5 
force, said residual body force, said resultant hinge accel- 
eration and said resultant hinge force each corresponds to a 
flexible and a rigid version thereof. 

9 . The method of claim 8 wherein said step of computing 
a resultant hinge force comprises computing the flexible 
version of said resultant hinge force from said specified 
body force, said flexible version of said residual body force 
and from said rigid version of said residual body force 
transformed by said modal spatial influence vector. 

10 . The method of claim 8 wherein said articulated body 
inertia comprises a rigid-flexible and rigid-rigid coupling 15 
components thereof, and wherein said method further com- 
prises the step of revising said rigid version of said residual 
body force based upon a function of said rigid-rigid and 
rigid-flexible coupling components of said articulated body 
inertia and a flexible version of said articulated body inertia 20 
to produce a revised rigid version of said residual body force 
for use in said correcting of said specified body forces. 

11 . The method of claim 10 wherein said computing said 
articulated body inertia step comprises decomposing said 
modal mass matrix into rigid-flexible and rigid-rigid cou- 25 
pling components and computing said rigid-rigid and rigid- 
flexible coupling components of said articulated body inertia 
from respective ones of said rigid-rigid and rigid-flexible 
coupling components of said modal mass matrix. 

12 . The method of claim 11 wherein said computing said 30 
articulated body quantities step is preceded by the step of 
computing flexible and rigid versions of a deformation and 
hinge modal joint map matrix for each plural body, and 
wherein: 

the flexible version of said articulated hinge inertia is 
computed from said articulated body inertia trans- 
formed by the flexible version of the corresponding 
deformation and hinge modal joint map matrix; 

the rigid version of said articulated body inertia is com- 
puted from a function of said rigid-rigid and rigid- 
flexible coupling components of said articulated body 
inertia transformed by said flexible version of said 
corresponding deformation and hinge modal joint map 
matrix; 

the rigid version of said articulated body inertia is com- 
puted from said rigid version of said articulated body 
inertia; 

the rigid version of said body to hinge force operator is 
computed from said rigid versions of said articulated 50 
body inertia and said articulated hinge inertia. 

13 . The method of claim 12 wherein said computing 
flexible and rigid versions of a deformation and hinge modal 
joint map matrix step comprises computing a joint map 
matrix corresponding to unit vectors of said movable hinges 55 
and computing said deformation and hinge modal joint 
matrix from said joint map matrix and from said modal 
spatial influence vector. 

14 . The method of claim 8 wherein the flexible version of 
said resultant hinge acceleration is computed from the 60 
flexible versions of said articulated hinge inertia and result- 
ant hinge force, and the rigid version of said resultant hinge 
acceleration is computed from the rigid versions of said 
articulated hinge inertia and resultant hinge force. 

15 . The method of claim 14 further comprising the step of 65 
revising said residual body force by adding to said residual 
body force a product of the rigid versions of said resultant 
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hinge force and said body to hinge force operator to produce 
a revised residual body force for use in said correcting of 
said specified body forces. 

16 . Apparatus for controlling a manipulator relative to a 
desired manipulator motion based upon specified body 
forces, said manipulator comprising plural bodies including 
an outermost body, and a relatively stationary innermost 
body, said plural bodies being sequentially connected 
together by movable hinges disposed between each plural 
body so connected and servos controlling said movable 
hinges in accordance with servo command signals corre- 
sponding to specified body forces of respective ones of said 
plural bodies, at least some of said plural bodies being 
flexible in plural deformation modes corresponding to 
respective modal spatial influence vectors relating deforma- 
tions of plural spaced nodes of respective plural bodies to 
said plural deformation modes, said apparatus comprising: 

means for computing articulated body quantities for each 
of said plural bodies from respective modal spatial 
influence vectors; 

means for computing modal deformation accelerations of 
said plural spaced nodes of respective plural bodies and 
hinge accelerations of said movable hinges from said 
specified body forces, from said articulated body quan- 
tities and from said modal spatial influence vectors; 

means for comparing said modal deformation and hinge 
accelerations with said desired manipulator motion so 
as to determine a motion discrepancy, and correcting 
said specified body forces so as to reduce said motion 
discrepancy; and 

means for generating said servo command signals by 
converting in a processor means said corrected speci- 
fied body forces to servo commands to correct manipu- 
lator motion to said desired manipulator motion, and 
transmitting said servo command signals to said servos. 

17 . The apparatus of claim 16 wherein said means for 
computing articulated body quantities comprises a means, 
operative for each plural body, beginning at said outermost 
body for: 

computing a modal mass matrix; 

computing an articulated body inertia from the articulated 
body inertia of a previous body and from said modal 
mass matrix; 

computing an articulated body to hinge force operator 
from said articulated hinge inertia; 

computing a null force operator from said articulated 
body to hinge force operator. 

18 . The method of claim 17 further comprising means for 
revising said articulated body inertia by transforming said 
articulated body inertia by said null force operator to pro- 
duce a revised articulated body inertia. 

19 . The apparatus of claim 17 wherein said plural bodies 
and movable hinges are characterized by respective vectors 
of deformation and hinge configuration variables, and 
wherein said means for computing modal deformation accel- 
erations and hinge accelerations comprise: 

means operative for each one of said plural bodies begin- 
ning with said outermost body, for: 
computing a residual body force from a residual body 
force of a previous body and from said vector of 
deformation and hinge configuration variables, 
computing a resultant hinge force from said specified 
body force and said residual body force, 
computing a resultant hinge acceleration from said 
resultant hinge force transformed by said articulated 
hinge inertia; 
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and, means operative for each one of said plural bodies 
beginning with said innermost body, for: 
computing a current modal body acceleration of a 
current body from a modal body acceleration of a 
previous body, 5 

computing a modal deformation acceleration and hinge 
acceleration from said resultant hinge acceleration 
and from said current modal body acceleration trans- 
formed by said articulated body to hinge force opera- 
tor. 10 

20. The apparatus of claim 19 further comprising: 

means for revising said residual body force by said 

resultant hinge force transformed by said body to hinge 
force operator to produce a revised residual body force 
for use in said correcting of said specified body forces; 15 
and 

means for revising said current modal body acceleration 
based upon said modal deformation and hinge accel- 
eration to produce a revised current modal body accel- 
eration for use in said correcting of said specified body 20 
forces. 

21. The apparatus of claim 20 wherein said means for 
computing modal deformation accelerations of said plural 
spaced nodes of respective plural bodies and hinge accel- 
erations of said movable hinges comprises means for com- 25 
puting said modal deformation accelerations and hinge 
accelerations once for each one of a succession of time steps, 
and wherein said means for computing modal deformation 
accelerations of said plural spaced nodes of respective plural 
bodies and hinge accelerations of said movable hinges 30 
further comprises means for computing said vector of defor- 
mation and hinge configuration variables from the modal 
deformations and hinge accelerations of a previous time step 
and wherein the revised articulated body inertia, revised 
residual body force and revised current modal body accel- 35 
eration from the previous time step are used for computing 
said modal deformation accelerations and hinge accelera- 
tions during a current time step. 

22. The apparatus of claim 19 further comprising means 
connected to joint sensors at each of said movable hinges for 40 
producing a hinge configuration portion of said vector of 
deformation and hinge configuration variables. 

23. The apparatus of claim 19 wherein said articulated 
body inertia, said articulated hinge inertia, said body to 
hinge force operator, said null force operator, said specified 45 
body force, said residual body force, said resultant hinge 
acceleration and said resultant hinge force each comprises at 
least one of a flexible and rigid version thereof. 

24. The apparatus of claim 23 wherein said means for 

computing a resultant hinge force comprises means for 50 
computing the flexible version of said resultant hinge force 
from said specified body force, said flexible version of said 
residual body force and from said rigid version of said 
residual body force transformed by said modal spatial influ- 
ence vector. 55 

25. The apparatus of claim 23 further comprising means 
for revising said rigid version of said residual body force 
based upon a function of rigid-rigid and rigid-flexible cou- 
pling components of said articulated body inertia and a 
flexible version of said articulated body inertia to produce a 60 
revised rigid version of said residual body force for use in 
said correcting of said specified body forces. 

26. The apparatus of claim 25 wherein said means for 
computing said articulated body inertia comprises means for 
decomposing said modal mass matrix into rigid-flexible and 65 
rigid-rigid coupling components and for computing said 
rigid-rigid and rigid-flexible coupling components of said 
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articulated body inertia from respective ones of said rigid- 
rigid and rigid-flexible coupling components of said modal 
mass matrix. 

27. The apparatus of claim 26 further comprising means 
for computing flexible and rigid versions of a deformation 
and hinge modal joint map matrix for each plural body, and 
further comprising: 

a means for computing the flexible version of said articu- 
lated hinge inertia, comprising means for computing 
the flexible version of said articulated hinge inertia 
from said articulated body inertia transformed by the 
flexible version of the corresponding deformation and 
hinge modal joint map matrix; 

a means for computing the rigid version of said articulated 
body inertia, comprising means for computing the rigid 
version of said articulated body inertia from a function 
of said rigid-rigid and rigid-flexible coupling compo- 
nents of said articulated body inertia transformed by 
said flexible version of said corresponding deformation 
and hinge modal joint map matrix; 

a means for computing the rigid version of said articulated 
hinge inertia, comprising means for computing the 
rigid version of said articulated hinge inertia from said 
rigid version of said articulated body inertia; 

a means for computing the rigid version of said body to 
hinge force operator, comprising means for computing 
the rigid version of said body to hinge force operator 
from said rigid versions of said articulated body inertia 
and said articulated hinge inertia. 

28. The apparatus of claim 27 wherein said means for 
computing flexible and rigid versions of a deformation and 
hinge modal joint map matrix comprises means for com- 
puting a joint map matrix corresponding to unit vectors of 
said movable hinges and means for computing said defor- 
mation and hinge modal joint map matrix from said joint 
map matrix and from said modal spatial influence vector. 

29. The apparatus of claim 23 further comprising 

a means for computing the flexible version of said result- 
ant hinge acceleration from the flexible versions of said 
articulated hinge inertia and resultant hinge force, and 

a means for computing the rigid version of said resultant 
hinge acceleration from the rigid versions of said 
articulated hinge inertia and resultant hinge force. 

30. The apparatus of claim 29 further comprising means 
for revising said residual body force by adding to said 
residual body force a product of the rigid versions of said 
resultant hinge force and said body to hinge force operator 
to create a revised residual body force for use in said 
correcting of said specified body forces. 

31. A manipulator controller for a manipulator responsive 
to specified body forces, said manipulator comprising plural 
bodies including an outermost body, and an innermost body, 
said plural bodies being sequentially connected together by 
movable hinges, disposed between each plural body so 
connected and servos controlling said movable hinges in 
accordance with servo command signals corresponding to 
specified body forces of respective ones of said plural 
bodies, at least some of said plural bodies being flexible in 
plural deformation modes corresponding to respective 
modal spatial influence vectors relating deformations of 
plural spaced nodes of respective plural bodies to said plural 
deformation modes, said manipulator controller comprising: 

means for computing articulated body quantities for each 
of said plural bodies from respective modal spatial 
influence vectors; 

means for computing modal deformation accelerations of 
said plural spaced nodes of respective plural bodies and 
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hinge accelerations of said movable hinges from said 
specified body forces, from said articulated body quan- 
tities and from said modal spatial influence vectors; 

means for comparing said modal deformation and hinge 
accelerations with said desired manipulator motion so 
as to determine a motion discrepancy, and correcting 
said specified body forces so as to reduce said motion 
discrepancy; and 

means for generating said servo command signals by 
converting in a processor means said corrected speci- 
fied body forces to servo commands to correct manipu- 
lator motion to a desired manipulator motion, and 
transmitting said servo command signals to said servos. 

32. The apparatus of claim 31 wherein said means for 
computing articulated body quantities comprises a means, 
operative for each plural body, beginning at said outermost 
body for: 

computing a modal mass matrix; 

computing an articulated body inertia from the articulated 
body inertia of a previous body and from said modal 
mass matrix; 

computing an articulated hinge inertia from said articu- 
lated body inertia; 

computing an articulated body to hinge force operator 
from said articulated hinge inertia; 

computing a null force operator from said articulated 
body to hinge force operator. 

33. The method of claim 32 further comprising means for 
revising said articulated body inertia by transforming said 
articulated body inertia by said null force operator to pro- 
duce a revised articulated body inertia. 

34. The apparatus of claim 32 wherein said plural bodies 
and movable hinges are characterized by respective vectors 
of deformation and hinge configuration variables, and 
wherein said means for computing modal deformation accel- 
erations and hinge accelerations comprise: 

means operative for each one of said plural bodies begin- 
ning with said outermost body, for: 
computing a residual body force from a residual body 
force of a previous body and from said vector of 
deformation and hinge configuration variables, 
computing a resultant hinge force from said specified 
body force and said residual body force, 
computing a resultant hinge acceleration from said 
resultant hinge force transformed by said articulated 
hinge inertia; 

and, means operative for each one of said plural bodies 
beginning with said innermost body, for: 
computing a current modal body acceleration of a 
current body from a modal body acceleration of a 
previous body, 

computing a modal deformation acceleration and hinge 
acceleration from said resultant hinge acceleration 
and from said current modal body acceleration trans- 
formed by said articulated body to hinge force opera- 
tor. 

35. The apparatus of claim 34 further comprising: 

means for revising said residual body force by said 

resultant hinge force transformed by said body to hinge 
force operator to produce a revised residual body force 
for use in said correcting of said specified body forces; 
and 

means for revising said current modal body acceleration 
based upon said modal deformation and hinge accel- 
eration to produce a revised current modal body accel- 
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eration for use in said correcting of said specified body 
forces. 

36. The apparatus of claim 35 wherein said means for 
computing modal deformation accelerations of said plural 

5 spaced nodes of respective plural bodies and hinge accel- 
erations of said movable hinges comprises means for com- 
puting said modal deformation accelerations and hinge 
accelerations once for each one of a succession of time steps, 
and wherein said means for computing modal deformation 
10 accelerations of said plural spaced nodes of respective plural 
bodies and hinge accelerations of said movable hinges 
further comprises means for computing said vector of defor- 
mation and hinge configuration variables from the modal 
deformations and hinge accelerations of a previous time step 
and wherein the revised articulated body inertia, revised 
15 residual body force and revised current modal body accel- 
eration from the previous time step are used for computing 
said modal deformation accelerations and hinge accelera- 
tions during a current time step. 

37. The apparatus of claim 34 further comprising means 
20 connected to joint sensors at each of said movable hinges for 

producing a hinge configuration portion of said vector of 
deformation and hinge configuration variables. 

38. The apparatus of claim 34 wherein said articulated 
body inertia, said articulated hinge inertia, said body to 

25 hinge force operator, said null force operator, said specified 
body force, said residual body force, said resultant hinge 
acceleration and said resultant hinge force each comprises at 
least one of a flexible and rigid version thereof. 

39. The apparatus of claim 38 wherein said means for 
30 computing a resultant hinge force comprises means for 

computing the flexible version of said resultant hinge force 
from said specified body force, said flexible version of said 
residual body force and from said rigid version of said 
residual body force transformed by said modal spatial infiu- 
35 ence vector. 

40. The apparatus of claim 38 further comprising means 
for revising said rigid version of said residual body force 
based upon a function of rigid-rigid and rigid-flexible cou- 
pling components of said articulated body inertia and a 

40 flexible version of said articulated body inertia to produce a 
revised rigid version of said residual body force for use in 
said correcting of said specified body forces. 

41. The apparatus of claim 40 wherein said means for 
computing said articulated body inertia comprises means for 

45 decomposing said modal mass matrix into rigid-flexible and 
rigid-rigid coupling components and for computing said 
rigid-rigid and rigid-flexible coupling components of said 
articulated body inertia from respective ones of said rigid- 
rigid and rigid-flexible coupling components of said modal 
50 mass matrix. 

42. The apparatus of claim 41 further comprising means 
for computing flexible and rigid versions of a deformation 
and hinge modal joint map matrix for each plural body, and 
further comprising: 

55 a means for computing the flexible version of said articu- 
lated hinge inertia, comprising means for computing 
the flexible version of said articulated hinge inertia 
from said articulated body inertia transformed by the 
flexible version of the corresponding deformation and 
60 hinge modal joint map matrix; 

a means for computing the rigid version of said articulated 
body inertia, comprising means for computing the rigid 
version of said articulated body inertia from a function 
of said rigid-rigid and rigid- flexible coupling compo- 
65 nents of said articulated body inertia transformed by 
said flexible version of said corresponding deformation 
and hinge modal joint map matrix; 
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a means for computing the rigid version of said articulated 
hinge inertia, comprising means for computing the 
rigid version of said articulated hinge inertia from said 
rigid version of said articulated body inertia; 

a means for computing the rigid version of said body to 5 
hinge force operator, comprising means for computing 
the rigid version of said body to hinge force operator 
from said rigid versions of said articulated body inertia 
and said articulated hinge inertia. 

43. The apparatus of claim 42 wherein said means for 10 
computing flexible and rigid versions of a deformation and 
hinge modal joint map matrix comprises means for com- 
puting a joint map matrix corresponding to unit vectors of 
said movable hinges and means for computing said defor- 
mation and hinge modal joint map matrix from said joint 15 
map matrix and from said modal spatial influence vector. 
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44. The apparatus of claim 38 further comprising 

a means for computing the flexible version of said result- 
ant hinge acceleration from the flexible versions of said 
articulated hinge inertia and resultant hinge force, and 

a means for computing the rigid version of said resultant 
hinge acceleration from the rigid versions of said 
articulated hinge inertia and resultant hinge force. 

45. The apparatus of claim 44 further comprising means 
for revising said residual body force by adding to said 
residual body force a product of the rigid versions of said 
resultant hinge force and said body to hinge force operator 
to create a revised residual body force for use in said 
correcting of said specified body forces. 



